引入

卡爾曼濾波是通過對下一時(shí)刻系統(tǒng)的狀態(tài)估計(jì)以及測量得出的反饋相結(jié)合，最終得到該時(shí)刻較為準(zhǔn)確的的狀態(tài)估計(jì)（預(yù)測+測量反饋），我們一般所說的卡爾曼濾波KF是針對線性系統(tǒng)的，其思想對于非線性系統(tǒng)也是適應(yīng)的，由此延伸出來的有EKF，UKF等。

我們先從簡單的問題入手，來了解卡爾曼濾波的思想

假如我們想知道一枚硬幣的厚度，并且有k個(gè)測量值，為了估計(jì)真實(shí)值，我們一般對其取均值，于是估計(jì)量

由①式知，當(dāng)我們只有一個(gè)測量值時(shí)，即k = 1，我們只能選擇相信測量值，而當(dāng)k —>∞時(shí)，我們可以相信上次的估計(jì)值。且本次的估計(jì)值只跟本次的測量值和上次的估計(jì)值有關(guān)，不用再追溯到前k-1個(gè)測量值

其中Kk為卡爾曼增益，為使估計(jì)誤差最小，我們使方差最小，令得，并做下式變換得卡爾曼增益的一般形式，此Kk值具有數(shù)學(xué)上的最優(yōu)性

eEST為估計(jì)誤差，eMEA為測量誤差。觀察②式知，當(dāng)e EST ?eMEA時(shí)，Kk = 1，將帶入①式，即當(dāng)估計(jì)誤差比較大時(shí)我們選擇相信測量值。相反，當(dāng)e EST ?eMEA時(shí)，Kk = 0，，此時(shí)我們選擇相信估計(jì)值

該情形下卡爾曼濾波的一般步驟：

卡爾曼濾波器

考慮系統(tǒng)

其中ω為過程噪聲，服從N(0,Q)分布。υ為測量噪聲，服從N(0,R)分布

1、協(xié)方差矩陣

將方差和協(xié)方差通過一個(gè)矩陣進(jìn)行表示

方差：

協(xié)方差：

考慮以下三階的協(xié)方差矩陣

構(gòu)造過度矩陣T求此協(xié)方差矩陣，構(gòu)成了的平均值矩陣

對于③式表示的系統(tǒng)

由于D(X) = E(X ^2^ ) - E ^2^ (X)，而E(ω_i) = 0，于是協(xié)方差矩陣可由下式表示

同時(shí)，ω 1 、ω 2 、… 、ωn相互獨(dú)立

同理R = E(υυ ^T^ )

2、先驗(yàn)估計(jì)

沒有考慮過程誤差和測量誤差，由狀態(tài)空間方程計(jì)算所得

但是在電機(jī)模型中ω和θ無法獲得，我們可以將替換為同時(shí)由測量得，

3、后驗(yàn)估計(jì)

估計(jì)+測量反饋

令G = KkH，則

4、卡爾曼增益

卡爾曼增益Kk是凹函數(shù)的極點(diǎn)，因此卡爾曼濾波器既是一個(gè)觀測器，又是最優(yōu)性估計(jì)

令，服從N(0,P)分布

為了使tr(P)最小，即方差最小，令得

具體推導(dǎo)過程可參考B站UP主DR_CAN，視頻鏈接放在文末

其中P ^-^ 為先驗(yàn)誤差

卡爾曼濾波的一般步驟

電機(jī)狀態(tài)方程

上文龍伯格觀測器中我們選取了狀態(tài)向量，但是為了利用卡爾曼濾波器對ω和θ進(jìn)行預(yù)測我們選取狀態(tài)向量，測量向量，并將反電動(dòng)勢形式做如下修改（電流滯后電壓90°，因此α軸的反電動(dòng)勢由β軸電壓產(chǎn)生，并且反向）

1、狀態(tài)向量

2、線性化

此時(shí)該系統(tǒng)是一個(gè)非線性系統(tǒng)，為了利用卡爾曼濾波我們要先對其進(jìn)行線性化，用EKF對系統(tǒng)狀態(tài)進(jìn)行估計(jì)，令

在先算先驗(yàn)估計(jì)時(shí)用的是未經(jīng)離散化的非線性函數(shù)

對上面卡爾曼計(jì)算一般步驟①先驗(yàn)估計(jì)做如下更正，其他步驟不變

雅可比矩陣

于是

3、離散化

于是線性化后的離散系統(tǒng)矩陣

MATALB仿真

EKF

可以看到卡爾曼濾波器的θ估計(jì)輸出幾乎與實(shí)際位置重合，速度估計(jì)ω受磁鏈常數(shù)影響，磁鏈給的偏大會(huì)導(dǎo)致ω估計(jì)偏小，相反磁鏈給的偏小會(huì)導(dǎo)致ω估計(jì)偏大，推薦對θ微分作為ω的估計(jì)