卡爾曼濾波的變種有哪些？2

6基于四元數(shù)的拓展卡爾曼濾波器

從實(shí)際問(wèn)題講起

在運(yùn)動(dòng)物體的姿態(tài)估計(jì)，比如車(chē)輛的姿態(tài)計(jì)算中，常常利用IMU(Inertial Meseasurment Unit)慣性測(cè)量單元計(jì)算物體的姿態(tài)。為了方便敘述，這里的姿態(tài)估計(jì)意味著我們希望解算車(chē)輛在每一時(shí)刻與起始坐標(biāo)系之間三個(gè)軸的偏轉(zhuǎn)角，通常用roll、pitch、yaw表示。

IMU(慣性測(cè)量單元)

現(xiàn)在的IMU一般是六軸或者九軸。六軸IMU可以輸出三個(gè)軸的加速度（acc）和角速度（gyro）；九軸則在六軸的基礎(chǔ)上增加了磁力計(jì)（Magnetometer），測(cè)量三軸的磁場(chǎng)方向。為了簡(jiǎn)化問(wèn)題，我們用六軸IMU作為示例。

四元素乘積的導(dǎo)數(shù)

這部分是為了后面推導(dǎo)擴(kuò)展卡爾曼濾波的狀態(tài)方程中的雅各比矩陣準(zhǔn)備

我們令時(shí)刻的旋轉(zhuǎn)為.時(shí)刻的旋轉(zhuǎn)為。則的旋轉(zhuǎn)是時(shí)刻的旋轉(zhuǎn)經(jīng)過(guò)的變化得到的，即

其中等于在時(shí)間間隔內(nèi)的角度變化量。在極短的時(shí)間間隔內(nèi)，我們認(rèn)為角度的變化是勻速的，也就是。我們令在這個(gè)時(shí)間間隔內(nèi)，角度的旋轉(zhuǎn)軸為，旋轉(zhuǎn)角度為,則

根據(jù)四元數(shù)的基礎(chǔ)知識(shí)，我們可以得到

擴(kuò)展卡爾曼濾波的重點(diǎn)之一在于求狀態(tài)方程相對(duì)于狀態(tài)的偏導(dǎo)；我們雖然可以從三軸角速度的輸出得出角度積分的離散形式公式（1），但是我們其實(shí)不能對(duì)其直接對(duì)求偏導(dǎo)。至于為什么不能直接求導(dǎo)，下面說(shuō)得挺好:

求導(dǎo)的定義是函數(shù)值的微增量關(guān)于自變量的微增量的極限。表示旋轉(zhuǎn)的單位四元數(shù)作差后，其不再是單位四元數(shù)，也就不是旋轉(zhuǎn)四元數(shù)了

為了能夠?qū)剑?）求出偏導(dǎo)數(shù)，我們先求旋轉(zhuǎn)相對(duì)時(shí)間的導(dǎo)數(shù)。然后利用泰勒展開(kāi)，將公式（1）展開(kāi)為線性方程，并只取一次項(xiàng)，這樣就可以得到

我們令

則

有了角度相對(duì)于時(shí)間的導(dǎo)數(shù)之后，我們可以將公式（1）用泰勒展開(kāi)

只保留到一次項(xiàng)，則我們可以得到

狀態(tài)向量及控制輸入

我們直接將車(chē)輛的姿態(tài)角以四元數(shù)形式表達(dá)作為系統(tǒng)狀態(tài)向量

同時(shí)，將每一時(shí)刻IMU的三軸角速度作為控制輸入

狀態(tài)方程及其雅各比矩陣

有了上一部分關(guān)于四元素導(dǎo)數(shù)的推導(dǎo)，我們可以直接寫(xiě)出狀態(tài)方程如下

其中是隨機(jī)噪聲，其協(xié)方差矩陣為

預(yù)測(cè)方程

由于在上面的推導(dǎo)中已經(jīng)求出雅各比矩陣，所以預(yù)測(cè)方程可以直接根據(jù)拓展卡爾曼濾波公式寫(xiě)出來(lái)

測(cè)量更新

前文我們使用了角速度計(jì)的輸出作為控制輸入，并構(gòu)建了狀態(tài)方程和預(yù)測(cè)方程。IMU通常都會(huì)有加速度計(jì)輸出，這部分輸出可以用來(lái)作為測(cè)量量，并對(duì)預(yù)測(cè)的狀態(tài)進(jìn)行測(cè)量更新。我們先回顧測(cè)量更新中狀態(tài)向量的更新過(guò)程。

令加速度計(jì)的輸出為測(cè)量向量：