imu姿態(tài)估計(MEMS器件的四元數(shù)EKF濾波)

MEMS器件的四元數(shù)EKF濾波

1，關(guān)于Kalman濾波

本文應(yīng)用的是MEMS器件的IMU,姿態(tài)的表示為四元數(shù)形式（不是網(wǎng)上的單軸卡爾曼濾波！不過原理都是一樣的）
首先是卡爾曼的五條公式

預(yù)測部分
$X_{k|k-1}=F_kX_{k-1|k-1}+B_ku_k$
$P_{k|k-1}=F_kP_{k-1|k-1}F_k^T+Q_k$
卡爾曼增益
$K_k=P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T+R_k)^{-1}$
更新部分
$X_{k|k}=X_{k|k-1}+K_k(Z_k-H_kX_{k|k-1})$
$P_{k|k}=(I-K_kH_k)P_{k|k-1}$
其中
$Z=HX+V$
式中變量的含義：
$X--$ 狀態(tài)向量
$P--$ 狀態(tài)向量的協(xié)方差矩陣驱敲，表示狀態(tài)的不確定度（或精度）
$X_{k|k}--$ $X$ 在 $k$ 時刻的最優(yōu)估計（后驗）
$X_{k|k-1}--$ 用 $k-1$ 時刻估計的狀態(tài)量（先驗）
$P_{k|k}--$ 后驗協(xié)方差矩陣
$P_{k|k-1}--$ 預(yù)測協(xié)方差矩陣
$B_k--$ 控制矩陣
$u_k--$ 控制向量
$Q_k--$ 預(yù)測過程的噪聲
$K_k--$ $k$ 時刻的卡爾曼增益
$H_k--$ $k$ 時刻狀態(tài)轉(zhuǎn)移矩陣
$Z_k--$ $k$ 時刻觀測向量
$R_k--$ 觀測噪聲

2耻讽，關(guān)于EKF

卡爾曼濾波是由線性系統(tǒng)推導(dǎo)來的，為了應(yīng)用于非線性系統(tǒng)，我們可以把非線性系統(tǒng)在平衡點附近線性化，這樣就有了EKF（擴展卡爾曼濾波）

在預(yù)測部分
$X_{k|k-1}=f(X_{k-1|k-1},u_k)$
由于 $f$ 是非線性的函數(shù)，我們把它在一點展開求它的 $Jacobians$ 矩陣
$F_k=\frac{\partial f}{\partial x}\bigg{|}_{X_{k-1|k-1},u_k}$
有時轉(zhuǎn)移矩陣也需要線性化
$\begin{align} Z=h(X)+V \end{align}$
$H_k=\frac{\partial h}{\partial x}\bigg{|}_{X_{k|k-1}}$
這樣可以寫成：
$Z=HX+V$

3眯分，實例化

現(xiàn)在我們要以MEMS的6DOF的imu器件為例看看怎么把EKF應(yīng)用到以四元數(shù)為基礎(chǔ)的姿態(tài)解算上，首先考慮下我們的器件特性骆莹，現(xiàn)認為陀螺儀在靜差校正后的輸出數(shù)學(xué)模型為：
$\begin{bmatrix} g_x\\ g_y \\ g_z\\ \end{bmatrix} = \begin{bmatrix} \omega_x+\delta_x \\ \omega_y+\delta_y \\\omega_z+ \delta_z \end{bmatrix}$
即認為陀螺儀的輸出值符合正態(tài)分布（當(dāng)然真實模型會復(fù)雜的多颗搂，不止包含白噪聲）
$g = N(\omega,\delta)$
先把上面這個式子放一下，現(xiàn)在我們選取四元數(shù)作為機體的姿態(tài)狀態(tài)量

$Q=\begin{bmatrix} q0 \\ q1 \\ q2 \\q3\end{bmatrix}$
然后我們要知道四元數(shù)的微分方程幕垦，以便更新四元數(shù)
$\dot{Q} = \begin{bmatrix} \dot{q0} \\ \dot{q1} \\ \dot{q2} \\ \dot{q3} \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0&-\omega_x&-\omega_y&-\omega_z\\ -\omega_x&0&-\omega_z&-\omega_y\\ \omega_y&-\omega_z&0&\omega_x\\ -\omega_z&\omega_y&-\omega_z&0 \end{bmatrix} \begin{bmatrix} q0\\q1\\q2\\q3 \end{bmatrix} \tag{1-1}$
我們可以用一階龍格-庫卡法來做數(shù)值積分丢氢，當(dāng)然可以用更高階的如二階傅联，四階龍格-庫卡法，這里用最簡單的一階（也就是矩形積分的形式）
$Q(T+t)=Q(t)+T\dot{Q}(t)$
現(xiàn)在我們得到了狀態(tài)轉(zhuǎn)移矩陣
$F_K = \frac{T}{2} \begin{bmatrix} 2&-\omega_x&-\omega_y&-\omega_z\\ -\omega_x&2&-\omega_z&-\omega_y\\ \omega_y&-\omega_z&2&\omega_x\\ -\omega_z&\omega_y&-\omega_x&2 \end{bmatrix} \tag{1-2}$
但注意到（1-2）中 $\omega$ 的真值是不知道的疚察，而我們把陀螺儀的數(shù)據(jù)帶進去是有誤差的蒸走，下面我們需要定義協(xié)方差矩陣 $P$ 來描述狀態(tài)向量的誤差。
$P= E \left[ (X-E[X])(X-E[X])^T \right]$
$P_{i,j}=cov(x_i,x_j) \tag{2-1}$
很明顯協(xié)方差矩陣 $P$ 是一個4x4的方陣貌嫡， $Q$ 的四個元素一定不是相互獨立的比驻，在也是為什么用協(xié)方差矩陣來表示狀態(tài)向量的分布情況。我們先把（2）式初始化為零岛抄，回到（1-2）討論一下預(yù)測過程的噪聲 $Q_k$ 該如何表示
由前面的討論可以知道：
$\begin{bmatrix}q0_{k|k-1} \\ q1_{k|k-1} \\q2_{k|k-1} \\q3_{k|k-1}\end{bmatrix}=\frac{T}{2}\begin{bmatrix} 2&-\omega_{x|k}&-\omega_{y|k}&-\omega_{z|k}\\ -\omega_{x|k}&2&-\omega_{z|k}&-\omega_{y|k}\\ \omega_{y|k}&-\omega_{z|k}&2&\omega_{x|k}\\ -\omega_{z|k}&\omega_{y|k}&-\omega_{x|k} &2 \end{bmatrix} \begin{bmatrix} q0_{k-1|k-1} \\ q1_{k-1|k-1} \\q2_{k-1|k-1} \\q3_{k-1|k-1} \end{bmatrix}$
現(xiàn)在我們把這個矩陣乘出來别惦，以第一行為例：
$q0_{k|k-1}=\frac{T}{2}(2q0_{k-1|k-1}-q1_{k-1|k-1}\omega_{x|k}-q2_{k-1|k-1}\omega_{y|k}-q3_{k-1|k-1}\omega_{z|k}) \tag{2-2}$
現(xiàn)在我們考慮現(xiàn)實情況把陀螺儀的輸出值 $g$ 代替 $\omega$
前面說明了 $g=N(\omega,\delta)$ ，于是上式即為正太分布概率密度函數(shù)的四則運算問題夫椭，我們認為 $gyro_x,gyro_y,gyro_z$ 是互相獨立的掸掸，這一點是非常重要的。
假設(shè) $X_{1},X_{2}$ 是符合正態(tài)分布的概率密度函數(shù) $X_{1}=N(\mu_1,\sigma_1^2) ,X_{2}=N(\mu_2,\sigma_2^2)$ $t$ 是一個常數(shù)蹭秋。那么：
$tX_{1}=N(t\mu_1,t^2\sigma_1^2)$
$X_{1}$ 與 $X_{2}$ 相互獨立時：
$X_{1}+X_{2}=N(\mu_1+\mu_2,\sigma_1^2+\sigma_2^2)$
回到式（2-2）我們可以把它變成：
$\begin{cases} q0_{k|k-1} = N(\frac{T}{2}(2q0_{k-1|k-1}-q1_{k-1|k-1}g_{x|k}-q2_{k-1|k-1}g_{y|k}-q3_{k-1|k-1}g_{z|k}),\sigma_0^2) \\ \sigma_0^2 = (\frac{T}{2})^2(q1_{k-1|k-1}^2\sigma_x^2 + q2_{k-1|k-1}^2\sigma_y^2 + q3_{k-1|k-1}^2\sigma_z^2 ) \end{cases}$
式中 $gyro_x=N(g_x,\sigma_x^2)\\gyro_y=N(g_y,\sigma_y^2)\\gyro_z=N(g_z,\sigma_z^2)$
綜上我們可以寫出預(yù)測過程的噪聲
$Q_k=\begin{bmatrix} Q_1&0&0&0 \\0&Q_2&0&0 \\0&0&Q_3&0 \\0&0&0&Q_4 \end{bmatrix} \tag{2-3}$
其中
$\begin{cases}Q_1=\sigma_0^2 = (\frac{T}{2})^2(q1_{k-1|k-1}^2\sigma_x^2 + q2_{k-1|k-1}^2\sigma_y^2 + q3_{k-1|k-1}^2\sigma_z^2 ) \\Q_2=\sigma_1^2 = (\frac{T}{2})^2(q0_{k-1|k-1}^2\sigma_x^2 + q2_{k-1|k-1}^2\sigma_z^2 + q3_{k-1|k-1}^2\sigma_y^2 ) \\Q_3=\sigma_2^2 = (\frac{T}{2})^2(q0_{k-1|k-1}^2\sigma_y^2 + q1_{k-1|k-1}^2\sigma_z^2 + q3_{k-1|k-1}^2\sigma_x^2 ) \\Q_4=\sigma_3^2 = (\frac{T}{2})^2(q0_{k-1|k-1}^2\sigma_z^2 + q1_{k-1|k-1}^2\sigma_y^2 + q2_{k-1|k-1}^2\sigma_x^2 ) \end{cases}$
ok現(xiàn)在我們的預(yù)測過程結(jié)束了扰付！下面我們要用傳感器的觀測來修正預(yù)測，在此之前我們先來討論下轉(zhuǎn)換矩陣 $H_k$
我們的觀測量往往與我們選擇的狀態(tài)量不是同一個單位甚至不是同一個維度仁讨，因此需要引入轉(zhuǎn)換矩陣
$Z=HX+V$
$Z$ 是我們的觀測向量
$Z=\begin{bmatrix} a_x^b \\ a_y^b \\a_z^b\end{bmatrix}$
表示機體坐標(biāo)系下重力加速度的分量
$V$ 是觀測噪聲羽莺，我們同樣認為是白噪聲（事實上忽略了機體運動的加速度)
根據(jù)慣性導(dǎo)航的知識（參考秦永元《慣性導(dǎo)航》）
$Z=\begin{bmatrix} a_x^b \\ a_y^b \\a_z^b\end{bmatrix} = \begin{bmatrix} 2g(q1q3 - q0q2)\\2g(q0q1 + q2q3)\\g(q0^2 + q3^2 - q1^2 - q2^2) \end{bmatrix}+V=h(X)+V$
我們在 $X = 0$ 點處將 $h(X)$ 線性化
$H_k = \frac{\partial h}{\partial x}\bigg{|}_{X_{k|k-1}=0}=2\begin{bmatrix} -q2_{k|k-1}&q3_{k|k-1} & -q0_{k|k-1} & q1_{k|k-1}\\q1_{k|k-1}&q0_{k|k-1}&q3_{k|k-1}&q2_{k|k-1}\\q0_{k|k-1}& -q1_{k|k-1}& -q2_{k|k-1}&q3_{k|k-1}\\ \end{bmatrix}\tag{3-1}$
現(xiàn)在我們看下整個狀態(tài)轉(zhuǎn)換的過程：

$\begin{bmatrix} a_x^b \\ a_y^b \\a_z^b \end{bmatrix}= 2 \begin{bmatrix} -q2_{k|k-1}&q3_{k|k-1} & -q0_{k|k-1} & q1_{k|k-1}\\q1_{k|k-1}&q0_{k|k-1}&q3_{k|k-1}&q2_{k|k-1}\\q0_{k|k-1}& -q1_{k|k-1}& -q2_{k|k-1}&q3_{k|k-1}\\ \end{bmatrix} \begin{bmatrix} q0_{k|k-1}\\q1_{k|k-1} \\q2_{k|k-1}\\q3_{k|k-1} \end{bmatrix}$
與 $Q_k$ 相同我們可以寫出觀測噪聲矩陣也就是加速度計噪聲 $R_k$
$R_k=\begin{bmatrix}\sigma_{ax}^2 & 0 & 0\\ 0 & \sigma_{ay}^2 & 0\\0 & 0 & \sigma_{az}^2\end{bmatrix}$
好了，到此所有卡爾曼需要的變量都推出來了洞豁，接下來只要按照卡爾曼的后三條公式依次計算卡爾曼增益盐固，然后更新狀態(tài)向量，再更新協(xié)方差矩陣就完成了一次迭代族跛。

4闰挡，一些tips

1.由于機體振動的影響，加速度計的 $R_k$ 可取變量礁哄，在加速度變化劇烈時 $R_k$ 取大些
2.對于資源有限的MCU來說，矩陣運算非常耗時溪北，可以把矩陣乘開桐绒，含零的項就可以不寫到代碼中了(不過這樣犧牲了可讀性)

5，結(jié)語

后續(xù)的筆記再慢慢補充吧之拨！
如有錯誤歡迎指出

最后編輯于：2019.03.31 15:32:42

?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者

人面猴
序言：七十年代末茉继，一起剝皮案震驚了整個濱河市，隨后出現(xiàn)的幾起案子蚀乔，更是在濱河造成了極大的恐慌烁竭，老刑警劉巖，帶你破解...
沈念sama閱讀 206,482評論 6贊 481
死咒
序言：濱河連續(xù)發(fā)生了三起死亡事件吉挣，死亡現(xiàn)場離奇詭異派撕，居然都是意外死亡婉弹，警方通過查閱死者的電腦和手機，發(fā)現(xiàn)死者居然都...
沈念sama閱讀 88,377評論 2贊 382
救了他兩次的神仙讓他今天三更去死
文/潘曉璐我一進店門终吼，熙熙樓的掌柜王于貴愁眉苦臉地迎上來镀赌，“玉大人，你說我怎么就攤上這事际跪∩谭穑” “怎么了？”我有些...
開封第一講書人閱讀 152,762評論 0贊 342
道士緝兇錄：失蹤的賣姜人
文/不壞的土叔我叫張陵姆打，是天一觀的道長良姆。經(jīng)常有香客問我，道長幔戏，這世上最難降的妖魔是什么玛追？我笑而不...
開封第一講書人閱讀 55,273評論 1贊 279
?港島之戀（遺憾婚禮）
正文為了忘掉前任，我火速辦了婚禮评抚，結(jié)果婚禮上豹缀，老公的妹妹穿的比我還像新娘。我一直安慰自己慨代，他們只是感情好邢笙，可當(dāng)我...
茶點故事閱讀 64,289評論 5贊 373
惡毒庶女頂嫁案：這布局不是一般人想出來的
文/花漫我一把揭開白布。她就那樣靜靜地躺著侍匙，像睡著了一般氮惯。火紅的嫁衣襯著肌膚如雪。梳的紋絲不亂的頭發(fā)上想暗，一...
開封第一講書人閱讀 49,046評論 1贊 285
城市分裂傳說
那天妇汗，我揣著相機與錄音，去河邊找鬼说莫。笑死杨箭，一個胖子當(dāng)著我的面吹牛，可吹牛的內(nèi)容都是我干的储狭。我是一名探鬼主播互婿，決...
沈念sama閱讀 38,351評論 3贊 400
雙鴛鴦連環(huán)套：你想象不到人心有多黑
文/蒼蘭香墨我猛地睜開眼，長吁一口氣：“原來是場噩夢啊……” “哼辽狈！你這毒婦竟也來了慈参？” 一聲冷哼從身側(cè)響起，我...
開封第一講書人閱讀 36,988評論 0贊 259
萬榮殺人案實錄
序言：老撾萬榮一對情侶失蹤刮萌，失蹤者是張志新（化名）和其女友劉穎驮配，沒想到半個月后，有當(dāng)?shù)厝嗽跇淞掷锇l(fā)現(xiàn)了一具尸體，經(jīng)...
沈念sama閱讀 43,476評論 1贊 300
?護林員之死
正文獨居荒郊野嶺守林人離奇死亡壮锻，尸身上長有42處帶血的膿包…… 初始之章·張勛以下內(nèi)容為張勛視角年9月15日...
茶點故事閱讀 35,948評論 2贊 324
?白月光啟示錄
正文我和宋清朗相戀三年琐旁，在試婚紗的時候發(fā)現(xiàn)自己被綠了。大學(xué)時的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片躯保。...
茶點故事閱讀 38,064評論 1贊 333
活死人
序言：一個原本活蹦亂跳的男人離奇死亡旋膳，死狀恐怖，靈堂內(nèi)的尸體忽然破棺而出途事，到底是詐尸還是另有隱情验懊，我是刑警寧澤，帶...
沈念sama閱讀 33,712評論 4贊 323
?日本核電站爆炸內(nèi)幕
正文年R本政府宣布尸变，位于F島的核電站义图，受9級特大地震影響，放射性物質(zhì)發(fā)生泄漏召烂。R本人自食惡果不足惜碱工，卻給世界環(huán)境...
茶點故事閱讀 39,261評論 3贊 307
男人毒藥：我在死后第九天來索命
文/蒙蒙一、第九天我趴在偏房一處隱蔽的房頂上張望奏夫。院中可真熱鬧怕篷，春花似錦、人聲如沸酗昼。這莊子的主人今日做“春日...
開封第一講書人閱讀 30,264評論 0贊 19
一樁弒父案，背后竟有這般陰謀
文/蒼蘭香墨我抬頭看了看天上的太陽麻削。三九已至蒸痹，卻和暖如春，著一層夾襖步出監(jiān)牢的瞬間呛哟，已是汗流浹背叠荠。一陣腳步聲響...
開封第一講書人閱讀 31,486評論 1贊 262
情欲美人皮
我被黑心中介騙來泰國打工，沒想到剛下飛機就差點兒被人妖公主榨干…… 1. 我叫王不留扫责，地道東北人榛鼎。一個月前我還...
沈念sama閱讀 45,511評論 2贊 354
代替公主和親
正文我出身青樓，卻偏偏與公主長得像鳖孤，于是被迫代替她去往敵國和親借帘。傳聞我的和親對象是個殘疾皇子，可洞房花燭夜當(dāng)晚...
茶點故事閱讀 42,802評論 2贊 345