淺析卡爾曼濾波原理

2021年6月17日9時(shí)22分，搭載神舟十二號(hào)載人飛船的運(yùn)載火箭，在酒泉衛(wèi)星發(fā)射中心點(diǎn)火發(fā)射。此后，神舟十二號(hào)載人飛船與火箭成功分離，進(jìn)入預(yù)定軌道，順利將聶海勝、劉伯明、湯洪波3名航天員送入太空，發(fā)射取得圓滿成功。

在載人飛船運(yùn)行的過程中，需要嚴(yán)格觀測及控制其運(yùn)行軌跡。談到載人航天，不得不提及人類歷史上第一次登月，阿波羅登月，阿波羅號(hào)成功的背后是運(yùn)用了卡爾曼濾波。

那什么是濾波？濾波是指從含有干擾的接收信號(hào)中提取有用信號(hào)的一種技術(shù)，是一種增強(qiáng)有用信號(hào)的數(shù)字信號(hào)處理過程。在數(shù)據(jù)通信中，無用信號(hào)表現(xiàn)為特定波段頻率和雜波，通常是一個(gè)隨機(jī)過程?？柭鼮V波是最優(yōu)濾波中的一種常用算法，是以實(shí)現(xiàn)信號(hào)或狀態(tài)的最優(yōu)估值與相應(yīng)的真實(shí)值的誤差的方差最小。

最早實(shí)現(xiàn)卡爾曼濾波器的是斯坦利·施密特。魯?shù)婪颉た柭谠L問NASA埃姆斯研究中心時(shí)，將斯密特的方法應(yīng)用于阿波羅計(jì)劃的軌道預(yù)測，并于1960年，提出離散系統(tǒng)卡爾曼濾波，后又將該種方法推廣到連續(xù)時(shí)間系統(tǒng)中。

正是由于卡爾曼濾波從時(shí)域分析處理數(shù)據(jù)，不僅可以處理平穩(wěn)隨機(jī)過程，也可以處理非平穩(wěn)隨機(jī)過程，所以被廣泛應(yīng)用于各種領(lǐng)域，例如慣性導(dǎo)航，定位系統(tǒng)，經(jīng)濟(jì)投資等。

卡爾曼濾波分類

常見的卡爾曼濾波主要分為離散型卡爾曼濾波和連續(xù)型卡爾曼濾波。

A.離散型卡爾曼濾波

遞推算法的使用是卡爾曼濾波的一大優(yōu)點(diǎn)，因此離散型卡爾曼濾波在工程上得到了廣泛的運(yùn)用。在一個(gè)濾波周期內(nèi)，根據(jù)卡爾曼濾波對(duì)使用系統(tǒng)信息和量測信息的先后次序，可以將卡爾曼濾波劃分為時(shí)間更新過程和量測更新過程，主要分為以下幾個(gè)部分。

1.一步預(yù)測：根據(jù)k-1時(shí)刻的狀態(tài)估計(jì)預(yù)測k時(shí)刻的狀態(tài)，通過k-1個(gè)時(shí)刻的量測量對(duì)k時(shí)刻狀態(tài)量進(jìn)行線性最小方差估計(jì)。

2.狀態(tài)估計(jì)方程：通過利用殘差（新息），對(duì)狀態(tài)量進(jìn)行修正估計(jì)

其中，是對(duì)殘差的加權(quán)陣，被稱為濾波增益陣。

3.濾波增益陣和估計(jì)均方誤差陣的推導(dǎo)：最佳濾波增益陣使得估計(jì)的均方誤差達(dá)到最小。

其中，是最佳增益陣，是最小均方誤差陣。

4.一步預(yù)測均方誤差陣推導(dǎo)

離散型卡爾曼濾波基本方程具有如下優(yōu)點(diǎn)：

（1）遞推算法的使用，使得數(shù)據(jù)的存儲(chǔ)量減少，通過濾波步數(shù)的增加，提取出的信息量在增大。

（2）只須知道驅(qū)動(dòng)噪聲的統(tǒng)計(jì)特性，使得卡爾曼濾波能對(duì)非平穩(wěn)的被估計(jì)量做估計(jì)。

B．連續(xù)型卡爾曼濾波

連續(xù)型卡爾曼濾波可以在離散型卡爾曼濾波的基礎(chǔ)上進(jìn)行推導(dǎo)，通過將連續(xù)系統(tǒng)離散化，應(yīng)用離散型卡爾曼濾波基本方程和導(dǎo)數(shù)運(yùn)算推導(dǎo)出連續(xù)型濾波方程。

設(shè)連續(xù)系統(tǒng)的系統(tǒng)方程和量測方程的一般形式為

連續(xù)型卡爾曼濾波的預(yù)測和更新的一般方程為：

非線性系統(tǒng)卡爾曼濾波算法

A．?dāng)U展卡爾曼濾波

上述卡爾曼濾波問題都是假設(shè)物理系統(tǒng)的數(shù)學(xué)模型是線性的，但是線性模型只是理想化，實(shí)際工程中很多的模型都是非線性的。同時(shí)，在非線性系統(tǒng)中，如果系統(tǒng)噪聲和觀測噪聲都是任意分布，那么系統(tǒng)的處理難度會(huì)更大。因此，一般會(huì)對(duì)噪聲的統(tǒng)計(jì)特性給出符合實(shí)際而又便于數(shù)學(xué)處理的設(shè)定。非線性系統(tǒng)的數(shù)學(xué)模型如下

其中，w（t）和v（t）是彼此不相關(guān)的零均值白噪聲序列，它們與初始狀態(tài)X（0）也不相關(guān)。

針對(duì)擴(kuò)展卡爾濾波，利用非線性函數(shù)的局部線性特性，將非線性模型局部線性化，通常是將非線性函數(shù)f圍繞濾波值做一階Taylor展開。

此外，將非線性函數(shù)h圍繞濾波值做一階Taylor展開。

線性化后，對(duì)其運(yùn)用線性卡爾曼濾波基本方程，便可獲得擴(kuò)展卡爾曼濾波遞推方程。

B．無跡卡爾曼濾波

擴(kuò)展卡爾曼濾波是對(duì)非線性的系統(tǒng)方程或者觀測方程進(jìn)行泰勒展開并保留一階近似項(xiàng)，引入了線性化誤差。無跡卡爾曼濾波摒棄了對(duì)非線性函數(shù)進(jìn)行線性化的傳統(tǒng)做法，采用卡爾曼線性濾波框架，對(duì)于一步預(yù)測方程，使用無跡變換來處理均值和協(xié)方差的非線性傳遞問題。

什么是無跡變換呢？在原狀態(tài)分布中按照某一規(guī)則選取一些采樣點(diǎn)，他們的均值和協(xié)方差等于原狀態(tài)分布的均值和協(xié)方差；并將他們帶入非線性函數(shù)，獲取相應(yīng)函數(shù)值的點(diǎn)集。對(duì)這些點(diǎn)求取變換后的均值和協(xié)方差。

針對(duì)無跡卡爾曼濾波，其非線性系統(tǒng)的描述

根據(jù)上述無跡變換，對(duì)非線性系統(tǒng)進(jìn)行變換處理。不同于在估計(jì)點(diǎn)處做Taylor級(jí)數(shù)展開，再進(jìn)行n階近似，而是在估計(jì)點(diǎn)附件進(jìn)行無跡變換，使得Sigma點(diǎn)集的均值和協(xié)方差與原統(tǒng)計(jì)特性匹配，再對(duì)這些Sigma點(diǎn)集進(jìn)行非線性映射，獲得狀態(tài)概率密度函數(shù)，實(shí)現(xiàn)一種統(tǒng)計(jì)近似。

C.交互式多模型卡爾曼濾波

針對(duì)前面幾種卡爾曼濾波，不必知道目標(biāo)的運(yùn)動(dòng)模型就能夠?qū)崟r(shí)修正目標(biāo)的狀態(tài)參量，具有較好的適應(yīng)性，但是如果目標(biāo)實(shí)施突然的運(yùn)動(dòng)變化時(shí)，基本的卡爾曼濾波就無法獲取好的結(jié)果了。交互式多模型卡爾曼濾波是一種軟切換算法，使用兩個(gè)或者更多的模型描述工作過程中的狀態(tài)，利于減小單模型估計(jì)誤差較大的影響。

交互式多模型卡爾曼濾波的思想是在每一個(gè)時(shí)刻，假設(shè)某個(gè)模型在當(dāng)下時(shí)刻有效，然后混合前一時(shí)刻所有濾波器的狀態(tài)估計(jì)值，獲取與這個(gè)特定模型匹配的濾波器的初始條件，然后對(duì)每個(gè)模型并行實(shí)施濾波，然后，以模型匹配似然函數(shù)為基礎(chǔ)更新模型概率，并組合所有濾波器修正后的狀態(tài)估計(jì)值以得到狀態(tài)估計(jì)。

在使用該算法時(shí)，對(duì)于濾波器的目標(biāo)運(yùn)動(dòng)模型的選擇，需要從以下幾個(gè)方面進(jìn)行考慮。

1.濾波器個(gè)數(shù)的選擇，包括較為精確的模型和較為粗糙的模型。

2.馬爾科夫鏈狀態(tài)轉(zhuǎn)移概率的選取對(duì)濾波器的性能有較大影響，會(huì)直接影響模型誤差和模型概率估計(jì)的準(zhǔn)確性。

3.該算法具有模塊化的特性，當(dāng)使用的時(shí)候，無法預(yù)料目標(biāo)的運(yùn)動(dòng)規(guī)律時(shí)，應(yīng)該選擇一般的魯棒性較強(qiáng)的模型。

作者：凌霄

浙江大學(xué)機(jī)械電子專業(yè)博士，從事智能傳感與人機(jī)交互，智能機(jī)器人控制等領(lǐng)域的研究

編輯：jq