詳解卡爾曼濾波

阿新 • • 發佈：2018-11-13

輕鬆一下看一個例子：

一片綠油油的草地上有一條曲折的小徑，通向一棵大樹。一個要求被提出：從起點沿著小徑走到樹下。
“很簡單。” A說，於是他絲毫不差地沿著小徑走到了樹下。
現在，難度被增加了：蒙上眼。
“也不難，我當過特種兵。” B說，於是他歪歪扭扭地走到了樹 ………. 旁。“唉，好久不練，生疏了。”
“看我的，我有 DIY 的 GPS！” C說，於是他像個醉漢似地走到了樹………. 旁。“唉，這個 GPS 軟體沒做好，漂移太大。”
“我來試試。” 旁邊一人拿過 GPS, 蒙上眼，居然沿著小徑走到了樹下。
“這麼厲害！你是什麼人?”
“卡爾曼 ! ”
“卡爾曼？！你是卡爾曼？”眾人大吃一驚。
“我是說這個 GPS 卡而慢。”
抄自

https://blog.csdn.net/bbs.21ic.com/icview-292853-1-1.html

綜述：

卡爾曼濾波是一種最優線性估計演算法而不是濾波器，其基於隱馬爾可夫模型，其動態系統可用一個馬爾科夫連結串列示，該馬爾科夫鏈鏈被高斯噪聲干擾。卡爾曼濾波分為兩個步驟：1.狀態預測。2狀態更新

最簡潔的方式介紹卡爾曼濾波：

圖片來自:https://zhuanlan.zhihu.com/p/25598462?utm_source=com.miui.notes&utm_medium=social

詳解：

卡爾曼濾波器的模型。圓圈代表向量，方塊代表

矩陣卡爾曼濾波模型假設k時刻的真實狀態是從（k − 1）時刻的狀態演化而來，符合下式：那麼真實狀態如下

${\textbf {x}}_{{k}}={\textbf {F}}_{{k}}{\textbf {x}}_{{k-1}}+{\textbf {B}}_{{k}}{\textbf {u}}_{{k}}+{\textbf {w}}_{{k}}$

其中

Fk是作用在xk−1上的狀態變換模型（/矩陣/向量）。
Bk是作用在控制器向量uk上的輸入－控制模型。
wk是過程噪聲，並假定其符合均值為零，協方差矩陣為Qk的多元正態分佈。

${\textbf {w}}_{{k}}\sim N(0,{\textbf {Q}}_{k})$

時刻k，對真實狀態xk的一個測量zk滿足下式：

${\textbf {z}}_{{k}}={\textbf {H}}_{{k}}{\textbf {x}}_{{k}}+{\textbf {v}}_{{k}}$

其中Hk是觀測模型，它把真實狀態空間對映成觀測空間，vk是觀測噪聲，其均值為零，協方差矩陣為Rk,且服從正態分佈。

${\textbf {v}}_{{k}}\sim N(0,{\textbf {R}}_{k})$

初始狀態以及每一時刻的噪聲{x0, w1, ..., w

k, v1 ... vk}都認為是互相獨立的。

實際上，很多真實世界的動態系統都並不確切的符合這個模型；但是由於卡爾曼濾波器被設計在有噪聲的情況下工作，一個近似的符合已經可以使這個濾波器非常有用了。更多其它更復雜的卡爾曼濾波器的變種。

卡爾曼濾波器詳解：

卡爾曼濾波是一種遞迴的估計，即只要獲知上一時刻狀態的估計值以及當前狀態的觀測值就可以計算出當前狀態的估計值，因此不需要記錄觀測或者估計的歷史資訊。卡爾曼濾波器與大多數濾波器不同之處，在於它是一種純粹的時域濾波器，它不需要像低通濾波器等頻域濾波器那樣，需要在頻域設計再轉換到時域實現。

卡爾曼濾波器的狀態由以下兩個變量表示：

${\hat {{\textbf {x}}}}_{{k|k}}$ ，在時刻k的狀態的估計；
${\textbf {P}}_{{k|k}}$ ，後驗估計誤差協方差矩陣，度量估計值的精確程度。

卡爾曼濾波器的操作包括兩個階段：預測與更新。在預測階段，濾波器使用上一狀態的估計，做出對當前狀態的估計。在更新階段，濾波器利用對當前狀態的觀測值優化在預測階段獲得的預測值，以獲得一個更精確的新估計值。

預測

${\hat {{\textbf {x}}}}_{{k|k-1}}={\textbf {F}}_{{k}}{\hat {{\textbf {x}}}}_{{k-1|k-1}}+{\textbf {B}}_{{k}}{\textbf {u}}_{{k}}$ (預測狀態)

${\textbf {P}}_{{k|k-1}}={\textbf {F}}_{{k}}{\textbf {P}}_{{k-1|k-1}}{\textbf {F}}_{{k}}^{{T}}+{\textbf {Q}}_{{k}}$ （預測估計協方差矩陣）

可參考：http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

更新

首先要算出以下三個量：

${\tilde {{\textbf {y}}}}_{{k}}={\textbf {z}}_{{k}}-{\textbf {H}}_{{k}}{\hat {{\textbf {x}}}}_{{k|k-1}}$ (測量餘量，measurement residual)

${\textbf {S}}_{{k}}={\textbf {H}}_{{k}}{\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}+{\textbf {R}}_{{k}}$ (測量餘量協方差)

${\textbf {K}}_{{k}}={\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}{\textbf {S}}_{{k}}^{{-1}}$ （最優卡爾曼增益）

然後用它們來更新濾波器變數x與P：

${\hat {{\textbf {x}}}}_{{k|k}}={\hat {{\textbf {x}}}}_{{k|k-1}}+{\textbf {K}}_{{k}}{\tilde {{\textbf {y}}}}_{{k}}$ (更新的狀態估計)

${\textbf {P}}_{{k|k}}=(I-{\textbf {K}}_{{k}}{\textbf {H}}_{{k}}){\textbf {P}}_{{k|k-1}}$ （更新的協方差估計）

使用上述公式計算 ${\textbf {P}}_{{k|k}}$ 僅在最優卡爾曼增益的時候有效。使用其他增益的話，公式要複雜一些，請參見推導。

不變數（Invariant）

如果模型準確，而且 ${\hat {{\textbf {x}}}}_{{0|0}}$ 與 ${\textbf {P}}_{{0|0}}$ 的值準確的反映了最初狀態的分佈，那麼以下不變數就保持不變：所有估計的誤差均值為零

${\textrm {E}}[{\textbf {x}}_{k}-{\hat {{\textbf {x}}}}_{{k|k}}]={\textrm {E}}[{\textbf {x}}_{k}-{\hat {{\textbf {x}}}}_{{k|k-1}}]=0$
${\textrm {E}}[{\tilde {{\textbf {y}}}}_{k}]=0$

且協方差矩陣準確的反映了估計的協方差：

${\textbf {P}}_{{k|k}}={\textrm {cov}}({\textbf {x}}_{k}-{\hat {{\textbf {x}}}}_{{k|k}})$
${\textbf {P}}_{{k|k-1}}={\textrm {cov}}({\textbf {x}}_{k}-{\hat {{\textbf {x}}}}_{{k|k-1}})$
${\textbf {S}}_{{k}}={\textrm {cov}}({\tilde {{\textbf {y}}}}_{k})$

請注意，其中 ${\textrm {E}}[{\textbf {a}}]$ 表示 ${a}$ 的期望值, ${\textrm {cov}}({\textbf {a}})={\textrm {E}}[{\textbf {a}}{\textbf {a}}^{T}]$ 。

補充：

https://blog.csdn.net/u010720661/article/details/63253509

詳解卡爾曼濾波

輕鬆一下看一個例子：

綜述：

最簡潔的方式介紹卡爾曼濾波：

詳解：

卡爾曼濾波器詳解：

詳解卡爾曼濾波

一個應用例項詳解卡爾曼濾波及其演算法實現

詳解卡爾曼濾波器

卡爾曼濾波詳解

mpu6050姿態解算與卡爾曼濾波（1）數學

數據會有一些波動和噪點采用卡爾曼濾波算法進行

卡爾曼濾波實現多項式擬合Matlab

卡爾曼濾波的一個簡單demo：恒定加速度模型

卡爾曼濾波3

卡爾曼濾波2

卡爾曼濾波

卡爾曼濾波的原理說明

【概率機器人】3.1 卡爾曼濾波、擴展卡爾曼濾波和無跡卡爾曼濾波

MATLAB-卡爾曼濾波簡單運用示例

cv2使用卡爾曼濾波（Kalman Filter）捕捉滑鼠運動

卡爾曼濾波MATLAB程式碼實現

卡爾曼濾波的理解以及推導過程

通俗理解卡爾曼濾波及其演算法實現（帶例項解析）

初學者的卡爾曼濾波——擴充套件卡爾曼濾波

卡爾曼濾波的總結

詳解卡爾曼濾波

輕鬆一下看一個例子：

綜述：

最簡潔的方式介紹卡爾曼濾波：

詳解：

卡爾曼濾波器詳解：

相關推薦