1 前言

在很多實(shí)際工程問題當(dāng)中，非線性系統(tǒng)占大多數(shù)，而卡爾曼提出來的卡爾曼濾波器是一種針對線性系統(tǒng)的估計(jì)算法[1]。 為了解決這一問題，Schmidt學(xué)者分析了卡爾曼濾波器中的系統(tǒng)模型部分，并提出將卡爾曼理論應(yīng)用到非線性系統(tǒng)的擴(kuò)展卡爾曼算法 Extended Kalman Filter(EKF)[2]。相比于線性卡爾曼濾波器，擴(kuò)展卡爾曼濾波器有更廣的適用范圍[3, 4]，更高的狀態(tài)估計(jì)精度，可以處理任意更新頻率的系統(tǒng)。 有了上文的基礎(chǔ)，這里，我們將通過一個(gè)二維小車的例子并來介紹擴(kuò)展卡爾曼濾波的原理。

2 擴(kuò)展卡爾曼濾波算法介紹

在介紹擴(kuò)展卡爾曼濾波算法之前，我們先回憶一下之前介紹的卡爾曼濾波算法：

其中前兩個(gè)公式為系統(tǒng)狀態(tài)預(yù)測部分，后三個(gè)公式為系統(tǒng)的修正部分。其中整個(gè)系統(tǒng)最重要的是要通過公式將系統(tǒng)的模型和測量與狀態(tài)之間的關(guān)系表示出來：

式中為系統(tǒng)矩陣，為控制矩陣，為觀測矩陣。為目標(biāo)在時(shí)刻的狀態(tài)，為控制量，和分別為系統(tǒng)誤差和測量誤差。在線性卡爾曼濾波算法中，和是常值矩陣，其對應(yīng)的系統(tǒng)就是線性系統(tǒng)。如果想將卡爾曼理論應(yīng)用到非線性系統(tǒng)中，則需要對非線性系統(tǒng)進(jìn)行線性化處理。

2.1 擴(kuò)展卡爾曼濾波算法的不同

非線性系統(tǒng)方程及其觀測方程通常可以表示為：

擴(kuò)展卡爾曼濾波算法可以通過將非線性系統(tǒng)在其參考點(diǎn)處做泰勒級數(shù)展開，從而使非線性系統(tǒng)可以線性化。展開后，我們可以得到：

其中和是非線性系統(tǒng)函數(shù)在時(shí)刻求偏導(dǎo)數(shù)后的雅可比矩陣，和為非線性測量函數(shù)求偏導(dǎo)數(shù)后的雅克比矩陣。那么，擴(kuò)展卡爾曼濾波算法可以寫成：

介紹完公式，接下來我們將通過2維平面內(nèi)的小車來對擴(kuò)展卡爾曼濾波算法進(jìn)行講解。

3 二維小車實(shí)例

如果要估計(jì)一個(gè)在二維平面內(nèi)運(yùn)動(dòng)的小車，首先要根據(jù)要求選擇一個(gè)較為合適的系統(tǒng)模型。在二維平面內(nèi)，小車的運(yùn)動(dòng)可能是直線也可能是曲線，那么在一維平面用的線性勻速模型constant velocity model(CV) 將不適用。這里將采用一個(gè)簡單的非線性模型勻轉(zhuǎn)速、速度模型 constant turning rate and velocity magnitude model(CTRV)。

3.1 非線性模型CTRV

ctrv模型是cv模型的一般形式。這里我們假設(shè)觀測到的狀態(tài)變量為：

分別對應(yīng)目標(biāo)在,方向上的位置，速度，角度和角速度，變量之間的關(guān)系如圖1所示。

圖 1: 二維平面ctrv模型

狀態(tài)量關(guān)系從圖中可以看出，各變量之間的相互關(guān)系。那么，從時(shí)刻到時(shí)刻，狀態(tài)的變化應(yīng)該是：

根據(jù)積分，我們可以得到下式：

這里，我們對系統(tǒng)函數(shù)求偏導(dǎo)得到其雅克比矩陣為

這里為了將系統(tǒng)誤差為：

后續(xù)再套入到卡爾曼濾波器中，我們就可以得到一個(gè)CTRV的跟蹤模型：

3.2 仿真

這里我們假設(shè)二維小車系統(tǒng)可以觀測到目標(biāo)的位置：

那么觀測方程為：

仿真總時(shí)間10s，時(shí)間間隔，目標(biāo)小車?yán)@原點(diǎn)，半徑以速度做勻速圓周運(yùn)動(dòng)。觀測信息為目標(biāo)的位置，測量誤差。

從視頻中我們可以看到，不同的模型即使在當(dāng)前估計(jì)狀態(tài)相差不大的情況下，其估計(jì)目標(biāo)未來的狀態(tài)也是有很大差別的。在這個(gè)例子中，目標(biāo)做的是勻速繞圈運(yùn)動(dòng)，CTRV模型更能描述系統(tǒng)的運(yùn)動(dòng)狀態(tài)，但該模型是非線性系統(tǒng)。因此，需要結(jié)合EKF，來實(shí)現(xiàn)對非線性系統(tǒng)的狀態(tài)跟蹤和預(yù)測。

仿真結(jié)果如圖2所示，紅色軌跡為小車真實(shí)軌跡，紅點(diǎn)為測量小車位置，藍(lán)色圓圈為CV模型估計(jì)的小車當(dāng)前位置，綠色圓圈為CTRV模型估計(jì)的小車當(dāng)前位置，藍(lán)色實(shí)線為CTRV模型估計(jì)小車軌跡，藍(lán)色虛線為CTRV模型預(yù)測未來一段時(shí)間內(nèi)的小車軌跡，綠色實(shí)線為CV模型估計(jì)小車軌跡，綠色虛線為CV模型預(yù)測未來一段時(shí)間內(nèi)的小車軌跡。可以看出CTRV模型的預(yù)測比CV模型的預(yù)測在繞圈運(yùn)動(dòng)中更為準(zhǔn)確。

4 總結(jié)

在很多實(shí)際工程問題中，系統(tǒng)的描述總存在不同程度的非線性，如平方、三角函數(shù)等。針對這一問題，EKF的思路是將非線性系統(tǒng)線性化轉(zhuǎn)換為近似的線性系統(tǒng)，然后應(yīng)用卡爾曼濾波算法完成狀態(tài)估計(jì)。

審核編輯：湯梓紅