卡爾曼濾波的五個公式

預測公式：

1.Xkp = AXk-1 + BUk + Wk

2.Pkp = APk-1A(轉置) + Qk

A：狀態轉移矩陣

B：控制矩陣

Wk：預測噪聲

Qk：狀態轉移噪聲

---------------------

狀態更新：

3.K = (Pkp*H)/(HPk-1H(轉置) + R)

4.Xk = Xkp + K(Yk - HXkp)

5.Pk = (I -KH)Pkp