注：受控制領(lǐng)域大牛CAN博士啟發(fā)，受益匪淺，作此文以為筆記。

簡介

??設(shè)

??卡爾曼濾波器是從測量值$Z$_k的平均數(shù)開始的。開始推導(dǎo)：

由上式可知

??也就是說隨著$k$的增大，測量結(jié)果Z_k不在重要，因為已經(jīng)獲得了足夠多的測量值，此時的估計值已經(jīng)很貼近了實際值了。我們令K_k=$1/k$，即

可知，K_k在$[0,1]$之間，當(dāng)K_k $=0$時，估計值等于上一次計算的估計值，當(dāng)K_k $=1$時，估計值等于本次測量值，這時引入兩個參數(shù)e_EST，e_MEA,令

有

其中，e_MEA是測量誤差，是測量工具自身的屬性，是不變的，e_EST是估計誤差，會受歷史數(shù)據(jù)的影響，即

由上述幾個式子便可使用卡爾曼濾波器來解決實際的問題了。步驟如下：
第一步

第二步

第三步

例
??有一個質(zhì)量為$50g$的物體，但我們此時并不知道該物體質(zhì)量是多少，先估計其有$46g$，估計誤差為$5g$，將其放在稱上稱得質(zhì)量為

該稱的測量誤差為$3g$,將所有數(shù)據(jù)放在Excel里進行計算

其中藍色線條表示測量值，紅色線條表示估計值，從圖中可以看出，盡管測量值起伏較大，但估計值整體趨勢很平緩，不斷向?qū)嶋H值靠攏且十分接近實際值。

數(shù)據(jù)融合

??從一個例子入手，設(shè)某物體質(zhì)量為$m$,分別用標(biāo)準差為σ₁ $=2g$和σ₂ $=4g$得稱來稱該物體，稱得質(zhì)量分別為Z₁ $=30g$和Z₂ $=33g$,求出最優(yōu)估計值。
??從上述中可得式

此時引入標(biāo)準差，即估計值的標(biāo)準差，當(dāng)標(biāo)準差越小時，即方差越小，估計值的波動越小，也就越趨于真實值。如下：

由上式可知，估計值的方差是關(guān)于K_k的函數(shù)，使估計值方差對K_k求導(dǎo)，即

將σ₁ $=2g$和σ₂ $=4g$代入上式中，K_k $=0.2$,得

協(xié)方差矩陣

??有以下$3$組數(shù)據(jù)

平均值

方差

協(xié)方差

協(xié)方差矩陣$P$

為方便編程計算，引入一個過渡矩陣$A$

則

注： 式中的$3$是指矩陣得維數(shù)。
??在$matlab$中驗證一下

與計算得結(jié)果一致。

狀態(tài)空間表達式

??有如下系統(tǒng)

??該系統(tǒng)中，物塊質(zhì)量為$M$,彈簧彈力系數(shù)為$k$,阻尼系數(shù)為$B$,系統(tǒng)輸入為拉力$F$。于是有

狀態(tài)變量

得

測量量

狀態(tài)空間表達式

化為離散形式

??由于系統(tǒng)存在各種不確定性，需要加入過程噪聲$W$和測量噪聲$V$,即

??$W$服從正態(tài)分布，期望為$0$，協(xié)方差矩陣為$Q$,即$P(W)?N(0,Q)$。$V$也服從正態(tài)分布，期望為$0$，協(xié)方差矩陣為$R$，即$P(V)?N(0,R)$。其中$Q=E[W{W}^T]$,推導(dǎo)如下：

同理，$R=E[V{V}^T]$。

卡爾曼增益推導(dǎo)

??由于過程噪聲是不確定的，于是狀態(tài)估計值先驗為

根據(jù)先驗估計和測量估計可得出后驗估計

令 $G=$ K_k$H$,則

??我們的目標(biāo)是求得合理的K_k值使得估計誤差最小，有

同理

??當(dāng)后驗估計值越接近真實值 X_k, 則說明 e_k 的方差越小，即 e_k 越接近于期望值$0$。于是有

接著推導(dǎo)

有

先驗誤差協(xié)方差矩陣

e_k的協(xié)方差矩陣P_k

??由之前的推導(dǎo)可得

總結(jié)

總結(jié)

以上是生活随笔為你收集整理的卡尔曼滤波器推导的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。