實作解讀 — 追蹤 1 維的最小實作

把目前為止的內容寫成程式碼，1 維卡爾曼濾波器就能寫得非常短。重要的不是「行數」，而是能追出每一行對應到哪個式子。

Python 最小實作

把狀態（x_hat、P）和參數（Q、R）整合在類別中。如果用 global 改寫模組變數，會導致無法同時運行多個濾波器、測試也較不方便，因此避免使用。

class KalmanFilter1D:
    def __init__(self, x_hat=0.0, P=10.0, Q=0.05, R=2.0):
        self.x_hat = x_hat
        self.P = P
        self.Q = Q
        self.R = R

    def step(self, z):
        x_pred = self.x_hat
        P_pred = self.P + self.Q

        K = P_pred / (P_pred + self.R)
        self.x_hat = x_pred + K * (z - x_pred)
        self.P = (1.0 - K) * P_pred
        return self.x_hat, self.P

寫成 (1.0 - K) 是為了避免被當成整數運算。Python 中即使寫 1 - K，只要 K 是 float，結果也會是 float，因此運作上相同；但這樣寫是為了與移植到 C 等其他語言時的行為一致，明確使用 1.0。

C 最小實作

C 版同樣將狀態與參數整合進結構體，函式接收結構體並對其進行寫入。不使用全域變數。

typedef struct {
    double x_hat;
    double P;
    double Q;
    double R;
} KalmanFilter1D;

double kf_step(KalmanFilter1D *kf, double z) {
    double x_pred = kf->x_hat;
    double P_pred = kf->P + kf->Q;

    double K = P_pred / (P_pred + kf->R);
    kf->x_hat = x_pred + K * (z - x_pred);
    kf->P = (1.0 - K) * P_pred;
    return kf->x_hat;
}

使用範例：以 KalmanFilter1D kf = { .x_hat = 0.0, .P = 10.0, .Q = 0.05, .R = 2.0 }; 初始化後，每次觀測都呼叫 kf_step(&kf, z)。

公式與變數的對應

解讀時的重點

1. 先把預測的 2 行與更新的 3 行分開
2. 確認 K 依賴於什麼
3. 最後確認變異數 P 是否正確更新