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