卡爾曼濾波器筆記

• 卡爾曼濾波器筆記
  • 1.基本動态系統模型
  • 2.卡爾曼濾波流程
    • 1.1推導最優卡爾曼增益
  • 3.擴充卡爾曼濾波

1.基本動态系統模型

狀态轉移 xk=Fkxk−1+Bkuk+wk x k = F k x k − 1 + B k u k + w k

測量模型 zk=Hkxk+vk z k = H k x k + v k

x x 表示狀态向量， u u 表示控制向量， z z 表示觀測向量， w w 表示過程噪聲， v v 表示觀測噪聲。 F F 表示狀态轉移模型， B B 表示控制-輸入模型， H H 表示觀測模型。下标 k k 及k−1k−1表示目前時刻以及上一時刻。

2.卡爾曼濾波流程

預測階段

預測（先驗）狀态估計 x̂ k|k−1=Fkx̂ k−1|k−1+Bkuk x ^ k | k − 1 = F k x ^ k − 1 | k − 1 + B k u k

預測（先驗）估計協方差 Pk|k−1=FkPk−1|k−1FTk+Qk P k | k − 1 = F k P k − 1 | k − 1 F k T + Q k

更新階段

計算測量餘量 ỹ k=zk−Hkx̂ k|k−1 y ~ k = z k − H k x ^ k | k − 1

計算測量餘量協方差 Sk=HkPk|k−1HT+Rk S k = H k P k | k − 1 H T + R k

計算最優卡爾曼增益 Kk=Pk|k−1HTkS−1k K k = P k | k − 1 H k T S k − 1

更新狀态估計 x̂ k|k=x̂ k|k−1+Kkỹ k x ^ k | k = x ^ k | k − 1 + K k y ~ k

更新估計協方差 Pk|k=(I−KkHk)Pk|k−1 P k | k = ( I − K k H k ) P k | k − 1

下标 k|k k | k 以及 k−1|k−1 k − 1 | k − 1 表示目前估計以及上一時刻估計。下标 k|k−1 k | k − 1 表示目前時刻的預測估計，是一種中間向量或模型。

卡爾曼濾波流程如下圖所示。

1.1推導最優卡爾曼增益

卡爾曼濾波器的最優化目的在于最小化估計狀态變量 x̂  x ^ 相對于真實狀态變量 x x 的均方誤差 E[|xk−x̂ k|k|] E [ | x k − x ^ k | k | ] ，等價于最小化估計協方差 Pk|k P k | k 的迹。其公式推導如下：

Pk|k=cov(xk−x̂ k|k)Pk|k=cov(xk−(x̂ k|k−1+Kk(Hkxk+vk−Hkx̂ k|k−1)))Pk|k=(I−KkHk)Pk|k−1(I−KkHk)T+KkRkKTk P k | k = c o v ( x k − x ^ k | k ) P k | k = c o v ( x k − ( x ^ k | k − 1 + K k ( H k x k + v k − H k x ^ k | k − 1 ) ) ) P k | k = ( I − K k H k ) P k | k − 1 ( I − K k H k ) T + K k R k K k T

dtr(Pk|k)dKk=−2(HkPk|k−1)T+2KkSk=0Kk=Pk|k−1HTkS−1k d t r ( P k | k ) d K k = − 2 ( H k P k | k − 1 ) T + 2 K k S k = 0 K k = P k | k − 1 H k T S k − 1

3.擴充卡爾曼濾波

基本卡爾曼濾波受限于線性模型假設。對于非線性模型，利用一階麥克勞林将其展開近似等效，展開公式中的常數部分對協方差的計算沒有貢獻，是以可利用Jacobians矩陣代替非線性模型計算協方差以及最優卡爾曼增益。