1. 程式人生 > >卡爾曼濾波的五個公式

卡爾曼濾波的五個公式

預測公式:

1.Xkp=AXk-1+Buk+wk

2.Pkp=APk-1AT+Qk

A:狀態轉移矩陣

B:控制矩陣

Wk:預測噪聲

Qk:狀態轉移噪聲

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

狀態更新:

3.K=PkpHTHPkpHT+R

 

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

5.Pk=(I-KH)Pkp