簡(jiǎn)介這份資源圍繞擴(kuò)展卡爾曼濾波EKF展開(kāi)面向從事慣性導(dǎo)航、姿態(tài)估計(jì)與多傳感器融合的開(kāi)發(fā)者及研究生重點(diǎn)解決9維IMU在姿態(tài)跟蹤中的濾波實(shí)現(xiàn)問(wèn)題。內(nèi)容參考《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU》論文給出基于EKF-IMU與ESKF-IMU兩套算法的實(shí)現(xiàn)代碼便于讀者對(duì)照論文理解狀態(tài)方程、觀測(cè)模型與誤差狀態(tài)更新的差異。壓縮包為zip格式整體約2.23MB文件類(lèi)型以源碼與工程文件為主可直接用于算法驗(yàn)證與二次開(kāi)發(fā)。目前已有441人學(xué)習(xí)下載適合希望從理論推導(dǎo)過(guò)渡到工程落地、需要現(xiàn)成參考實(shí)現(xiàn)來(lái)調(diào)試姿態(tài)解算效果的讀者也可作為課程設(shè)計(jì)或科研項(xiàng)目的起步素材。1. 擴(kuò)展卡爾曼濾波一個(gè)讓機(jī)器人不跑偏的“導(dǎo)航大腦”你有沒(méi)有想過(guò)掃地機(jī)器人在你家客廳里轉(zhuǎn)來(lái)轉(zhuǎn)去怎么就知道自己現(xiàn)在在哪兒、下一步該往哪走它沒(méi)有眼睛輪子還會(huì)打滑可它就是能一圈圈把地掃干凈。這背后有個(gè)東西在默默干活叫擴(kuò)展卡爾曼濾波英文縮寫(xiě) EKF。說(shuō)白了它就是一個(gè)“猜位置”的數(shù)學(xué)工具專(zhuān)門(mén)對(duì)付那些“不太聽(tīng)話”的系統(tǒng)——比如輪子會(huì)打滑、傳感器有噪聲、運(yùn)動(dòng)規(guī)律不是一條直線。普通卡爾曼濾波只能處理線性系統(tǒng)就像只能走直路而現(xiàn)實(shí)世界大多是彎的EKF 就是那個(gè)允許你走彎路的升級(jí)版。搞懂它你就明白為什么無(wú)人機(jī)能懸停、手機(jī)能定位、自動(dòng)駕駛能跟車(chē)——這些場(chǎng)景里EKF 幾乎無(wú)處不在。2. 為什么需要 EKF從“只能走直線”到“允許拐彎”2.1 卡爾曼濾波的局限線性假設(shè)太苛刻卡爾曼濾波誕生在上世紀(jì)六十年代一開(kāi)始是為了解決阿波羅登月里的導(dǎo)航問(wèn)題。它的核心思想很樸素你有一個(gè)“預(yù)測(cè)”還有一個(gè)“測(cè)量”兩者都不準(zhǔn)那就按各自的不確定度加權(quán)平均一下得到一個(gè)更準(zhǔn)的估計(jì)。這個(gè)思路在線性系統(tǒng)里是最優(yōu)的數(shù)學(xué)上漂亮得沒(méi)話說(shuō)。但問(wèn)題來(lái)了什么叫線性系統(tǒng)打個(gè)比方你開(kāi)車(chē)油門(mén)踩多少速度就漲多少方向打多少車(chē)頭就轉(zhuǎn)多少而且這個(gè)關(guān)系是成比例的、可疊加的?？涩F(xiàn)實(shí)里你踩油門(mén)速度不是線性漲的風(fēng)阻是平方關(guān)系你打方向車(chē)走的軌跡是圓弧角度一變位置變化就不是簡(jiǎn)單乘法了。這些“彎的”關(guān)系卡爾曼濾波處理不了硬套上去就會(huì)翻車(chē)。注意很多初學(xué)者以為卡爾曼濾波就是一個(gè)公式背下來(lái)就行。其實(shí)它是一套“預(yù)測(cè)-更新”的遞推框架線性假設(shè)是它的命門(mén)。2.2 EKF 的解法用“切線”把彎的掰直EKF 的做法很聰明既然整個(gè)曲線我搞不定那我就在當(dāng)前估計(jì)點(diǎn)附近用一條切線去近似它。這條切線就是“線性化”。數(shù)學(xué)上叫一階泰勒展開(kāi)你不用記這個(gè)名字只需要知道EKF 在每個(gè)時(shí)刻都把非線性的運(yùn)動(dòng)模型和觀測(cè)模型在當(dāng)前估計(jì)值附近“掰直”成線性的然后再套用卡爾曼濾波的那套加權(quán)平均。這就像你要測(cè)量一個(gè)彎曲山坡的坡度你不可能一次測(cè)完整座山但你可以站在當(dāng)前位置用一塊小直板貼住地面量出這一點(diǎn)的坡度。EKF 就是不斷在移動(dòng)中做這件事每一步都重新“貼板子”所以它能跟著系統(tǒng)走。代價(jià)是什么線性化會(huì)引入誤差。如果系統(tǒng)彎得太厲害或者你估計(jì)的點(diǎn)離真實(shí)值太遠(yuǎn)切線就偏得離譜EKF 可能發(fā)散——也就是越估越錯(cuò)最后徹底跑飛。這也是為什么后來(lái)有人搞出了無(wú)跡卡爾曼濾波UKF和粒子濾波它們不用線性化但計(jì)算量更大。EKF 的定位就是在精度和計(jì)算量之間找一個(gè)平衡點(diǎn)夠用、快、好實(shí)現(xiàn)。2.3 一個(gè)生活化類(lèi)比蒙眼找路想象你被蒙上眼睛站在一個(gè)廣場(chǎng)上手里有個(gè) GPS 告訴你大概位置但誤差有十米。你每走一步心里默數(shù)步數(shù)和方向估算自己移動(dòng)了多少。GPS 讀數(shù)來(lái)了你就跟自己的估算對(duì)比一下如果 GPS 說(shuō)你在東邊你心里估算也在東邊那就信自己多一點(diǎn)如果 GPS 突然跳到西邊你心里估算沒(méi)動(dòng)那就懷疑 GPS 抽風(fēng)了少信它一點(diǎn)。EKF 干的就是這件事只不過(guò)它用數(shù)學(xué)嚴(yán)格定義了“信誰(shuí)多一點(diǎn)”——用協(xié)方差矩陣表示不確定度。你走得越久自己估算的不確定度越大就更信 GPSGPS 信號(hào)差的時(shí)候不確定度大就更信自己的推算。整個(gè)過(guò)程是遞歸的不需要存歷史數(shù)據(jù)每一步只依賴上一步的估計(jì)和當(dāng)前測(cè)量所以特別適合跑在單片機(jī)上。3. EKF 的核心零件預(yù)測(cè)、更新與雅可比矩陣3.1 預(yù)測(cè)步用運(yùn)動(dòng)模型往前推EKF 每個(gè)循環(huán)分兩步第一步叫預(yù)測(cè)。你有一個(gè)狀態(tài)向量比如機(jī)器人的位置 x、y 和朝向角 θ還有速度 v 和角速度 ω。根據(jù)上一時(shí)刻的狀態(tài)你用運(yùn)動(dòng)學(xué)公式推算這一時(shí)刻的狀態(tài)。如果是非線性函數(shù) f就直接代進(jìn)去算。同時(shí)你還要預(yù)測(cè)不確定度。不確定度用一個(gè)協(xié)方差矩陣 P 表示它描述了你對(duì)每個(gè)狀態(tài)量的信心。預(yù)測(cè)時(shí)P 會(huì)變大因?yàn)橥频迷竭h(yuǎn)越不準(zhǔn)。這里的關(guān)鍵是非線性函數(shù) f 不能直接乘 P需要先求它的雅可比矩陣 F。雅可比矩陣就是 f 對(duì)每個(gè)狀態(tài)量的偏導(dǎo)數(shù)組成的矩陣它告訴你在當(dāng)前點(diǎn)狀態(tài)微小變化會(huì)如何影響下一步。說(shuō)白了它就是“局部斜率”的多維版本。import numpy as np def predict(x, P, u, dt): # x: 狀態(tài)向量 [x, y, theta, v, omega] # u: 控制輸入這里假設(shè)為0實(shí)際可含加速度 # dt: 時(shí)間步長(zhǎng) theta x[2] v x[3] omega x[4] # 非線性運(yùn)動(dòng)模型 if abs(omega) 1e-6: x_new x[0] (v/omega) * (np.sin(theta omega*dt) - np.sin(theta)) y_new x[1] (v/omega) * (-np.cos(theta omega*dt) np.cos(theta)) else: x_new x[0] v * np.cos(theta) * dt y_new x[1] v * np.sin(theta) * dt theta_new theta omega * dt x_pred np.array([x_new, y_new, theta_new, v, omega]) # 雅可比矩陣 F對(duì)狀態(tài)量求偏導(dǎo) F np.eye(5) if abs(omega) 1e-6: F[0,2] (v/omega) * (np.cos(theta omega*dt) - np.cos(theta)) F[0,3] (np.sin(theta omega*dt) - np.sin(theta)) / omega F[0,4] (v/omega**2) * (np.sin(theta) - np.sin(theta omega*dt)) (v*dt/omega) * np.cos(theta omega*dt) F[1,2] (v/omega) * (np.sin(theta omega*dt) - np.sin(theta)) F[1,3] (-np.cos(theta omega*dt) np.cos(theta)) / omega F[1,4] (v/omega**2) * (np.cos(theta omega*dt) - np.cos(theta)) (v*dt/omega) * np.sin(theta omega*dt) else: F[0,2] -v * np.sin(theta) * dt F[0,3] np.cos(theta) * dt F[1,2] v * np.cos(theta) * dt F[1,3] np.sin(theta) * dt F[2,4] dt # 過(guò)程噪聲協(xié)方差 Q需要根據(jù)實(shí)際調(diào) Q np.diag([0.1, 0.1, 0.01, 0.1, 0.1]) P_pred F P F.T Q return x_pred, P_pred上面這段代碼里F就是雅可比矩陣Q是過(guò)程噪聲表示你對(duì)運(yùn)動(dòng)模型有多不信任。Q調(diào)大濾波器更信測(cè)量調(diào)小更信預(yù)測(cè)。這個(gè)參數(shù)沒(méi)有標(biāo)準(zhǔn)答案得根據(jù)輪子打滑程度、地面摩擦來(lái)試。3.2 更新步用測(cè)量修正預(yù)測(cè)預(yù)測(cè)完你有了一個(gè)先驗(yàn)估計(jì)。接下來(lái)傳感器來(lái)數(shù)據(jù)了比如激光雷達(dá)測(cè)到機(jī)器人離墻 1.2 米或者 GPS 給出經(jīng)緯度。你把預(yù)測(cè)的狀態(tài)代入觀測(cè)方程 h算出“如果我在預(yù)測(cè)的位置傳感器應(yīng)該看到什么”。然后拿這個(gè)預(yù)測(cè)的觀測(cè)值和實(shí)際觀測(cè)值相減得到殘差。殘差乘以卡爾曼增益 K就得到修正量。K 的大小取決于預(yù)測(cè)不確定度 P_pred 和測(cè)量噪聲 R 的比值。P_pred 大、R 小K 就大更信測(cè)量反之更信預(yù)測(cè)。更新完P(guān) 會(huì)變小因?yàn)槿诤狭诵碌男畔⑿判脑黾恿?。觀測(cè)方程 h 通常也是非線性的比如從位置算距離和角度涉及平方和開(kāi)方、反正切。所以更新步同樣需要 h 的雅可比矩陣 H。H 的每一行對(duì)應(yīng)一個(gè)觀測(cè)量每一列對(duì)應(yīng)一個(gè)狀態(tài)量元素是偏導(dǎo)數(shù)。def update(x_pred, P_pred, z, R): # z: 實(shí)際觀測(cè) [range, bearing] # R: 測(cè)量噪聲協(xié)方差 def h(x): px, py x[0], x[1] r np.sqrt(px**2 py**2) b np.arctan2(py, px) return np.array([r, b]) # 雅可比矩陣 H px, py x_pred[0], x_pred[1] r np.sqrt(px**2 py**2) H np.zeros((2, 5)) H[0,0] px / r H[0,1] py / r H[1,0] -py / (r**2) H[1,1] px / (r**2) z_pred h(x_pred) y z - z_pred # 角度殘差歸一化到 [-pi, pi] y[1] (y[1] np.pi) % (2*np.pi) - np.pi S H P_pred H.T R K P_pred H.T np.linalg.inv(S) x_new x_pred K y P_new (np.eye(5) - K H) P_pred return x_new, P_new這段代碼里H是觀測(cè)雅可比R是測(cè)量噪聲。角度殘差歸一化那一步很容易被忽略但如果不做角度從 179 度跳到 -179 度時(shí)殘差會(huì)變成 358 度濾波器直接懵掉。這是血淚經(jīng)驗(yàn)新手必踩。3.3 雅可比矩陣EKF 的“方向盤(pán)”雅可比矩陣在 EKF 里扮演的角色就像方向盤(pán)在車(chē)?yán)锏慕巧?。它不直接決定你去哪但它決定了你對(duì)“當(dāng)前該往哪修正”的判斷。如果雅可比算錯(cuò)了濾波器就會(huì)往錯(cuò)誤方向修正輕則收斂慢重則發(fā)散。很多人第一次寫(xiě) EKF運(yùn)動(dòng)模型和觀測(cè)模型都寫(xiě)對(duì)了但結(jié)果就是不對(duì)最后發(fā)現(xiàn)是雅可比求導(dǎo)漏了一項(xiàng)或者符號(hào)搞反了。建議的做法是先用數(shù)值差分驗(yàn)證雅可比。比如對(duì)每個(gè)狀態(tài)量加一個(gè)小擾動(dòng) ε算函數(shù)值變化再除以 ε跟解析雅可比對(duì)比。如果誤差在 1e-6 量級(jí)說(shuō)明推導(dǎo)正確。提示數(shù)值差分驗(yàn)證雅可比是調(diào)試 EKF 最有效的手段沒(méi)有之一。花十分鐘寫(xiě)個(gè)驗(yàn)證腳本能省你十小時(shí)抓頭發(fā)。4. 易混概念EKF、KF、UKF、粒子濾波到底怎么分4.1 卡爾曼濾波KF與 EKF線性與非線性KF 和 EKF 最核心的區(qū)別就一個(gè)系統(tǒng)是不是線性的。KF 要求運(yùn)動(dòng)模型和觀測(cè)模型都是矩陣乘法形式x_{k1} A x_k B u_kz_k C x_k。EKF 允許 f 和 h 是任意非線性函數(shù)代價(jià)是每步要算雅可比而且只是局部近似。什么時(shí)候用 KF如果你的系統(tǒng)本身就是線性的比如勻速直線運(yùn)動(dòng)、電路里的電壓電流關(guān)系那就用 KF計(jì)算量小而且是最優(yōu)估計(jì)。什么時(shí)候用 EKF只要模型里有三角函數(shù)、平方、開(kāi)方、指數(shù)基本就得 EKF。但注意EKF 不是全局最優(yōu)只是局部近似初值給得太離譜可能不收斂。4.2 EKF 與 UKF線性化 vs 采樣UKF 全稱無(wú)跡卡爾曼濾波它的思路跟 EKF 完全不同。EKF 是把非線性函數(shù)線性化UKF 是選一組確定性的采樣點(diǎn)叫 sigma 點(diǎn)把這些點(diǎn)代入非線性函數(shù)算變換后的均值和協(xié)方差。它不需要求雅可比而且對(duì)強(qiáng)非線性系統(tǒng)往往比 EKF 更準(zhǔn)。那為什么還用 EKF因?yàn)?UKF 計(jì)算量更大尤其是狀態(tài)維度高的時(shí)候sigma 點(diǎn)數(shù)量是 2n1n 是狀態(tài)維數(shù)。如果 n20就要算 41 個(gè)點(diǎn)每個(gè)點(diǎn)都要過(guò)一遍非線性函數(shù)。EKF 只需要算一個(gè)雅可比矩陣雖然求導(dǎo)麻煩但算起來(lái)快。所以嵌入式系統(tǒng)里 EKF 仍然很常見(jiàn)UKF 多用在精度要求高、算力也夠的場(chǎng)景。4.3 EKF 與粒子濾波高斯假設(shè)的邊界粒子濾波完全不假設(shè)分布是高斯它用一堆隨機(jī)樣本粒子來(lái)表示概率分布能處理多峰、非高斯的情況。比如機(jī)器人被綁架到另一個(gè)房間全局定位問(wèn)題粒子濾波就能處理EKF 直接歇菜因?yàn)樗挥幸粋€(gè)高斯峰沒(méi)法同時(shí)表示“我可能在 A 房間也可能在 B 房間”。但粒子濾波計(jì)算量巨大粒子少了精度不夠粒子多了跑不動(dòng)。而且它有隨機(jī)性每次跑結(jié)果可能略有不同。EKF 是確定性的給定初值和參數(shù)每次結(jié)果一樣便于調(diào)試和復(fù)現(xiàn)。所以選型邏輯是如果系統(tǒng)基本是單峰、近似高斯EKF 夠用且高效如果多峰、強(qiáng)非線性再考慮粒子濾波。4.4 常見(jiàn)誤區(qū)EKF 不是“更準(zhǔn)的 KF”很多人以為 EKF 是 KF 的升級(jí)版精度一定更高。這是個(gè)誤解。EKF 的精度取決于線性化誤差。如果系統(tǒng)非線性很強(qiáng)或者工作點(diǎn)離真實(shí)值很遠(yuǎn)EKF 可能比 KF 還差甚至發(fā)散。KF 在線性系統(tǒng)里是最優(yōu)的EKF 在非線性系統(tǒng)里只是“近似最優(yōu)”而且這個(gè)近似好不好得看具體問(wèn)題。另一個(gè)誤區(qū)是“Q 和 R 隨便設(shè)”。Q 是過(guò)程噪聲R 是測(cè)量噪聲它們直接決定卡爾曼增益影響濾波器的“性格”。Q 設(shè)太小濾波器太自信測(cè)量來(lái)了也不怎么修正響應(yīng)慢Q 設(shè)太大濾波器太敏感測(cè)量噪聲全放進(jìn)來(lái)輸出抖動(dòng)。調(diào) Q 和 R 是 EKF 實(shí)戰(zhàn)里最耗時(shí)間的環(huán)節(jié)沒(méi)有之一。5. 什么時(shí)候用 EKF什么時(shí)候別用一個(gè)具體例子假設(shè)你在做一個(gè)模擬項(xiàng)目讓一個(gè)小車(chē)在走廊里沿著墻走車(chē)上有一個(gè)輪速計(jì)和一個(gè)激光雷達(dá)。輪速計(jì)給出速度但輪子會(huì)打滑激光雷達(dá)測(cè)距但偶爾有反射噪聲。你想估計(jì)小車(chē)的位置和朝向。這個(gè)場(chǎng)景就是 EKF 的典型用武之地。狀態(tài)量是 x、y、θ運(yùn)動(dòng)模型是非線性的因?yàn)?θ 在三角函數(shù)里觀測(cè)模型也是非線性的距離和角度。EKF 能跑在單片機(jī)上實(shí)時(shí)性沒(méi)問(wèn)題。你需要做的就是寫(xiě)運(yùn)動(dòng)模型、寫(xiě)觀測(cè)模型、求雅可比、調(diào) Q 和 R。但如果你把場(chǎng)景換成小車(chē)在整棟樓里找自己可能在任何樓層任何房間初始位置完全未知。這時(shí)候 EKF 就不合適了因?yàn)樗母咚辜僭O(shè)只能表示一個(gè)峰沒(méi)法同時(shí)覆蓋多個(gè)可能位置。你應(yīng)該用粒子濾波或者多假設(shè)跟蹤。再換一個(gè)場(chǎng)景小車(chē)在一條筆直的鐵軌上跑輪速計(jì)測(cè)速度GPS 測(cè)位置。這就是線性系統(tǒng)直接用 KF沒(méi)必要上 EKF。上 EKF 反而多算雅可比浪費(fèi)算力還可能因?yàn)榫€性化引入不必要的誤差。所以判斷標(biāo)準(zhǔn)很清晰第一系統(tǒng)是不是非線性的不是就用 KF。第二分布是不是近似單峰高斯不是就考慮粒子濾波。第三算力夠不夠夠且精度要求高可以試 UKF不夠就 EKF。這三個(gè)問(wèn)題問(wèn)完選型基本就定了。我自己的習(xí)慣是每次拿到一個(gè)新系統(tǒng)先花半小時(shí)把運(yùn)動(dòng)模型和觀測(cè)模型寫(xiě)成代碼用數(shù)值差分驗(yàn)證雅可比然后跑一段離線數(shù)據(jù)看殘差是不是零均值白噪聲。如果殘差有偏或者自相關(guān)說(shuō)明模型有問(wèn)題或者 Q、R 沒(méi)調(diào)好。這個(gè)流程走一遍能避開(kāi)大部分翻車(chē)現(xiàn)場(chǎng)。從那以后我每次上 EKF 之前都強(qiáng)制走一遍這個(gè)檢查再也沒(méi)出現(xiàn)過(guò)濾波器發(fā)散到外太空的情況。希望幫到你。本文還有配套的精品資源點(diǎn)擊獲取