簡(jiǎn)介這份資源圍繞擴(kuò)展卡爾曼濾波EKF展開面向從事慣性導(dǎo)航、姿態(tài)估計(jì)與多傳感器融合的開發(fā)者與研究者尤其適合已具備一定濾波理論基礎(chǔ)、希望將算法落地到9維IMU場(chǎng)景的中高級(jí)學(xué)習(xí)者。內(nèi)容基于論文《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU》實(shí)現(xiàn)涵蓋EKF-IMU與ESKF-IMU兩套姿態(tài)解算方案可用于無人機(jī)、可穿戴設(shè)備、機(jī)器人等方向的方向跟蹤與狀態(tài)估計(jì)驗(yàn)證。壓縮包為zip格式整體約2.23MB便于快速下載與本地部署。目前已有441人學(xué)習(xí)下載說明該實(shí)現(xiàn)具備一定的參考價(jià)值。讀者可從中獲取完整的算法實(shí)現(xiàn)代碼與工程組織思路對(duì)照論文理解雙階段卡爾曼濾波的推導(dǎo)過程并借助EKF與ESKF兩種實(shí)現(xiàn)的差異對(duì)比掌握誤差狀態(tài)建模、四元數(shù)更新與觀測(cè)修正等關(guān)鍵環(huán)節(jié)為自身項(xiàng)目中的姿態(tài)融合模塊提供可復(fù)用的參考方案。1. 從一次姿態(tài)估計(jì)翻車說起EKF 到底在算什么去年幫一個(gè)做飛控的朋友調(diào)四軸懸停時(shí)姿態(tài)角突然抽風(fēng)roll 軸在 3 秒內(nèi)漂了 15 度。日志拉出來一看陀螺儀積分發(fā)散加速度計(jì)噪聲又大得沒法直接用。他問我有沒有什么“后悔藥”我說你缺的不是藥是一個(gè)能把兩種爛數(shù)據(jù)揉成一份好數(shù)據(jù)的框架——擴(kuò)展卡爾曼濾波也就是 EKF。EKF 不是新東西但它是慣性導(dǎo)航、機(jī)器人定位、傳感器融合里繞不開的一環(huán)。簡(jiǎn)單說卡爾曼濾波處理的是線性系統(tǒng)而現(xiàn)實(shí)里的姿態(tài)動(dòng)力學(xué)、GPS/INS 組合導(dǎo)航、目標(biāo)跟蹤幾乎全是非線性的。EKF 的做法就是在每一步用一階泰勒展開把非線性函數(shù)在當(dāng)前估計(jì)點(diǎn)附近“掰直”然后套用標(biāo)準(zhǔn)卡爾曼濾波的那套預(yù)測(cè)-更新流程。它不保證全局最優(yōu)但在工程上足夠好用計(jì)算量也可控。這份資源適合誰如果你正在做 IMU 姿態(tài)解算、多傳感器融合、SLAM 前端或者單純想搞懂“為什么我的濾波器一跑就發(fā)散”那它值得你花時(shí)間拆一遍。下面我從數(shù)學(xué)骨架講到代碼落地再到我踩過的那些坑盡量讓新手能跟著跑熟手能看到邊界。2. EKF 的數(shù)學(xué)骨架從泰勒展開到協(xié)方差遞推2.1 非線性系統(tǒng)怎么“掰直”雅可比矩陣是核心標(biāo)準(zhǔn)卡爾曼濾波假設(shè)狀態(tài)轉(zhuǎn)移和觀測(cè)都是線性的x_k F x_{k-1} B u_k w_kz_k H x_k v_k。但真實(shí)系統(tǒng)里姿態(tài)更新涉及三角函數(shù)觀測(cè)可能是距離的平方根全是非線性。EKF 的思路是我不改函數(shù)我改“工作點(diǎn)”。具體做法是在當(dāng)前狀態(tài)估計(jì) x?_{k-1} 附近對(duì)非線性函數(shù) f(x) 和 h(x) 做一階泰勒展開f(x) ≈ f(x?_{k-1}) F_{k-1} (x - x?_{k-1}) h(x) ≈ h(x?_k) H_k (x - x?_k)其中 F 是 f 對(duì) x 的雅可比矩陣H 是 h 對(duì) x 的雅可比矩陣。這兩個(gè)矩陣就是 EKF 的“黑匣子”——它們把非線性問題局部線性化讓卡爾曼增益的推導(dǎo)還能用。我一般會(huì)先手推一遍雅可比再用符號(hào)計(jì)算工具驗(yàn)證。比如姿態(tài)估計(jì)里四元數(shù)更新對(duì)陀螺儀偏差的偏導(dǎo)手推容易漏項(xiàng)用 Python 的 sympy 跑一遍能省不少調(diào)試時(shí)間。import sympy as sp # 定義狀態(tài)變量四元數(shù) qw,qx,qy,qz 和陀螺儀偏差 bw,bx,by,bz qw, qx, qy, qz, bw, bx, by, bz sp.symbols(qw qx qy qz bw bx by bz) dt, wx, wy, wz sp.symbols(dt wx wy wz) # 陀螺儀測(cè)量減去偏差 gx wx - bx gy wy - by gz wz - bz # 四元數(shù)微分方程簡(jiǎn)化形式未歸一化 qw_dot -0.5 * (qx*gx qy*gy qz*gz) qx_dot 0.5 * (qw*gx qy*gz - qz*gy) qy_dot 0.5 * (qw*gy - qx*gz qz*gx) qz_dot 0.5 * (qw*gz qx*gy - qy*gx) # 一階歐拉積分后的狀態(tài)轉(zhuǎn)移 q_next [qw qw_dot*dt, qx qx_dot*dt, qy qy_dot*dt, qz qz_dot*dt] b_next [bw, bx, by, bz] # 計(jì)算雅可比 F d(state_next)/d(state) state [qw, qx, qy, qz, bw, bx, by, bz] state_next q_next b_next F sp.Matrix(state_next).jacobian(state) sp.pprint(F)這段代碼做了一件事把四元數(shù)運(yùn)動(dòng)學(xué)方程和偏差模型寫成符號(hào)表達(dá)式然后自動(dòng)求雅可比。邏輯上F 矩陣的左上 4x4 塊描述四元數(shù)如何隨自身和偏差變化右上 4x4 塊描述偏差對(duì)四元數(shù)更新的影響右下 4x4 是單位陣假設(shè)偏差隨機(jī)游走。參數(shù) dt 是積分步長(zhǎng)wx/wy/wz 是陀螺儀原始讀數(shù)。實(shí)際寫 C 代碼時(shí)我會(huì)把這個(gè)符號(hào)結(jié)果展開成顯式表達(dá)式避免在嵌入式里跑符號(hào)運(yùn)算。注意雅可比是在當(dāng)前估計(jì)點(diǎn)計(jì)算的所以每一步都要重新算。如果狀態(tài)維度高比如 15 維 INS 誤差狀態(tài)手寫雅可比容易出錯(cuò)建議用自動(dòng)微分工具生成后再固化。2.2 預(yù)測(cè)與更新五個(gè)方程走一遍有了 F 和 H剩下的就是標(biāo)準(zhǔn)卡爾曼五步。我把它拆成預(yù)測(cè)和更新兩段方便對(duì)照代碼。預(yù)測(cè)階段狀態(tài)預(yù)測(cè)x?_k? f(x?_{k-1}, u_k)協(xié)方差預(yù)測(cè)P_k? F_{k-1} P_{k-1} F_{k-1}^T Q更新階段 3. 卡爾曼增益K_k P_k? H_k^T (H_k P_k? H_k^T R)^{-1} 4. 狀態(tài)更新x?_k x?_k? K_k (z_k - h(x?_k?)) 5. 協(xié)方差更新P_k (I - K_k H_k) P_k?這里 Q 是過程噪聲協(xié)方差R 是觀測(cè)噪聲協(xié)方差。Q 和 R 的取值直接決定濾波器是“信模型”還是“信觀測(cè)”。我一般先用傳感器手冊(cè)里的噪聲密度初始化再根據(jù)實(shí)際跑出來的殘差調(diào)整。如果殘差白噪聲檢驗(yàn)不通過說明 Q 或 R 設(shè)錯(cuò)了。import numpy as np def ekf_predict(x, P, u, dt, Q): # 狀態(tài)預(yù)測(cè)這里用簡(jiǎn)化的勻速模型 F np.array([[1, dt], [0, 1]]) x_pred F x np.array([0.5*dt*dt, dt]) * u P_pred F P F.T Q return x_pred, P_pred, F def ekf_update(x_pred, P_pred, z, H, R): # 計(jì)算卡爾曼增益 S H P_pred H.T R K P_pred H.T np.linalg.inv(S) # 狀態(tài)更新 y z - H x_pred # 新息 x_upd x_pred K y # 協(xié)方差更新Joseph 形式數(shù)值更穩(wěn) I np.eye(len(x_pred)) P_upd (I - K H) P_pred (I - K H).T K R K.T return x_upd, P_upd, K, y這段代碼里ekf_predict 用勻速模型演示了預(yù)測(cè)步F 是 2x2 矩陣Q 需要根據(jù)實(shí)際加速度噪聲填。ekf_update 里我特意用了 Joseph 形式更新協(xié)方差而不是簡(jiǎn)單的 (I-KH)P。原因在后面避坑章節(jié)會(huì)講——簡(jiǎn)單形式在數(shù)值誤差累積下可能讓 P 失去正定性Joseph 形式對(duì)稱且更穩(wěn)。H 是觀測(cè)雅可比R 是觀測(cè)噪聲協(xié)方差y 是新息可以用來做異常檢測(cè)。參數(shù)說明dt 是采樣周期u 是控制輸入比如加速度z 是觀測(cè)向量。如果觀測(cè)是非線性的h(x?_k?) 要單獨(dú)算H 是 h 對(duì) x 的雅可比在當(dāng)前預(yù)測(cè)點(diǎn)的值。3. 把 EKF 塞進(jìn) IMU 姿態(tài)解算代碼落地與參數(shù)整定3.1 狀態(tài)量、觀測(cè)量與坐標(biāo)系約定拿六軸 IMU 做姿態(tài)估計(jì)常見狀態(tài)量有兩種選法四元數(shù) 陀螺儀偏差7 維或者旋轉(zhuǎn)矩陣 偏差12 維但約束多。我一般用四元數(shù)因?yàn)楦潞?jiǎn)單歸一化方便。觀測(cè)量用加速度計(jì)測(cè)重力方向如果有磁力計(jì)就加上測(cè)地磁方向。坐標(biāo)系約定必須一開始就定死機(jī)體坐標(biāo)系 b 系導(dǎo)航坐標(biāo)系 n 系東北天。加速度計(jì)在靜止時(shí)輸出的是比力歸一化后就是重力方向在 b 系的投影。EKF 的觀測(cè)方程 h(x) 就是旋轉(zhuǎn)矩陣乘以參考重力向量 [0,0,1]^T。def quat_to_rot(q): # q [w, x, y, z] w, x, y, z q return np.array([ [1-2*(y*yz*z), 2*(x*y-w*z), 2*(x*zw*y)], [2*(x*yw*z), 1-2*(x*xz*z), 2*(y*z-w*x)], [2*(x*z-w*y), 2*(y*zw*x), 1-2*(x*xy*y)] ]) def accel_observation(q): # 參考重力方向在 n 系為 [0,0,1] R quat_to_rot(q) return R.T np.array([0, 0, 1])quat_to_rot 把四元數(shù)轉(zhuǎn)成旋轉(zhuǎn)矩陣accel_observation 算出預(yù)測(cè)的加速度計(jì)讀數(shù)方向。實(shí)際比較時(shí)要把測(cè)量值歸一化再和預(yù)測(cè)值做差。如果加速度計(jì)動(dòng)態(tài)加速度大這個(gè)觀測(cè)就不可信需要調(diào)大 R 或者直接跳過更新。3.2 噪聲協(xié)方差 Q 和 R 怎么設(shè)從手冊(cè)到試湊Q 和 R 是 EKF 的“玄學(xué)”參數(shù)但也不是完全沒章法。陀螺儀手冊(cè)會(huì)給噪聲密度單位是 °/s/√Hz乘以采樣帶寬的平方根就是角度隨機(jī)游走。加速度計(jì)手冊(cè)給的是 mg/√Hz換算成 m/s2 后平方就是 R 的對(duì)角線。我一般這樣起步參數(shù)含義初始值來源調(diào)整方向Q_quat四元數(shù)過程噪聲陀螺儀噪聲密度 × dt姿態(tài)響應(yīng)慢就調(diào)大Q_bias偏差隨機(jī)游走陀螺儀零偏穩(wěn)定性偏差收斂慢就調(diào)大R_accel加速度計(jì)觀測(cè)噪聲手冊(cè)噪聲密度平方動(dòng)態(tài)加速度大就調(diào)大R_mag磁力計(jì)觀測(cè)噪聲手冊(cè)噪聲密度平方磁干擾大就調(diào)大實(shí)際跑的時(shí)候我會(huì)先讓 Q 小一點(diǎn)、R 大一點(diǎn)看濾波器是否跟得上。如果姿態(tài)滯后說明太信模型了調(diào)大 Q_quat。如果姿態(tài)跳變說明太信觀測(cè)了調(diào)大 R_accel。這個(gè)過程沒有捷徑但可以用新息序列做輔助判斷新息應(yīng)該是零均值白噪聲如果出現(xiàn)連續(xù)同號(hào)說明模型和觀測(cè)有系統(tǒng)偏差。# 初始化 q np.array([1.0, 0, 0, 0]) # 初始姿態(tài) bias np.zeros(3) # 陀螺儀偏差 P np.eye(6) * 0.1 # 初始協(xié)方差 Q np.diag([1e-4]*4 [1e-6]*3) # 四元數(shù)過程噪聲 偏差隨機(jī)游走 R_acc np.eye(3) * 0.01 # 加速度計(jì)觀測(cè)噪聲 # 一次迭代 gyro np.array([0.01, -0.02, 0.005]) # 陀螺儀讀數(shù) rad/s acc np.array([0.02, 0.01, 9.81]) # 加速度計(jì)讀數(shù) m/s2 acc_norm acc / np.linalg.norm(acc) # 預(yù)測(cè)四元數(shù)積分 協(xié)方差傳播 # ...雅可比 F 計(jì)算略見第 2 章 # 更新加速度計(jì)觀測(cè) # ...H 和 K 計(jì)算略這段偽代碼展示了初始化思路。Q 的前 4 個(gè)對(duì)角元對(duì)應(yīng)四元數(shù)過程噪聲后 3 個(gè)對(duì)應(yīng)偏差隨機(jī)游走。R_acc 初始給 0.01相當(dāng)于標(biāo)準(zhǔn)差 0.1 m/s2對(duì)靜止場(chǎng)景夠用。如果做動(dòng)態(tài)飛行R_acc 要調(diào)大一個(gè)量級(jí)。注意四元數(shù)有模長(zhǎng)約束每次更新后要?dú)w一化。但歸一化會(huì)引入微小誤差P 矩陣也要相應(yīng)調(diào)整。常見做法是歸一化后不調(diào) P靠 Joseph 形式保持?jǐn)?shù)值穩(wěn)定。3.3 從 Python 原型到 C 移植三個(gè)必須對(duì)齊的點(diǎn)Python 跑通后移植到嵌入式 C 是另一道坎。我踩過的坑集中在三點(diǎn)第一浮點(diǎn)精度。Python 默認(rèn)雙精度STM32 上如果用 float 單精度協(xié)方差矩陣在迭代幾百步后可能失去正定性。解決辦法是協(xié)方差更新用 Joseph 形式并且定期做對(duì)稱化P (P P.T) / 2。第二矩陣求逆。EKF 更新里要算 (H P H^T R)^{-1}這個(gè)矩陣維度通常很小觀測(cè)維度但嵌入式里沒有現(xiàn)成庫。我一般手寫 3x3 或 6x6 的求逆用伴隨矩陣除以行列式。注意行列式接近零時(shí)要加保護(hù)否則數(shù)值爆炸。第三三角函數(shù)查表。姿態(tài)更新涉及 sin/cos嵌入式里用查表比調(diào) math.h 快。但查表精度要和 dt 匹配否則引入的截?cái)嗾`差會(huì)累積。我一般用 1024 點(diǎn)表加線性插值精度夠用。// 3x3 矩陣求逆伴隨矩陣法 void mat3_inv(float A[3][3], float A_inv[3][3]) { float det A[0][0]*(A[1][1]*A[2][2] - A[1][2]*A[2][1]) - A[0][1]*(A[1][0]*A[2][2] - A[1][2]*A[2][0]) A[0][2]*(A[1][0]*A[2][1] - A[1][1]*A[2][0]); if (fabsf(det) 1e-12f) det 1e-12f; // 保護(hù) float inv_det 1.0f / det; A_inv[0][0] (A[1][1]*A[2][2] - A[1][2]*A[2][1]) * inv_det; // ... 其余元素類似 }這段 C 代碼展示了 3x3 求逆的保護(hù)邏輯。det 接近零時(shí)強(qiáng)制設(shè)一個(gè)小值避免除零。實(shí)際工程里我會(huì)在求逆前檢查矩陣條件數(shù)如果太大就跳過本次更新等觀測(cè)變好再說。4. 避坑與排查EKF 發(fā)散、跳變、收斂慢的五個(gè)血淚經(jīng)驗(yàn)4.1 現(xiàn)象協(xié)方差矩陣 P 失去正定性濾波器直接發(fā)散原因用簡(jiǎn)單形式 P (I - K H) P 更新浮點(diǎn)誤差累積后 P 出現(xiàn)負(fù)特征值?；蛘?Q 設(shè)得太小P 收縮到零附近卡爾曼增益失去調(diào)節(jié)能力。解決協(xié)方差更新改用 Joseph 形式 P (I-KH) P (I-KH)^T K R K^T這個(gè)形式對(duì)任意 K 都保持對(duì)稱正定。另外定期做 P (P P.T)/2 對(duì)稱化。如果已經(jīng)發(fā)散把 P 重置為初始值Q 調(diào)大一個(gè)量級(jí)重新收斂。4.2 現(xiàn)象姿態(tài)在動(dòng)態(tài)機(jī)動(dòng)時(shí)跳變靜止時(shí)正常原因加速度計(jì)在動(dòng)態(tài)時(shí)測(cè)的是比力不是純重力。如果 R_accel 設(shè)得太小濾波器會(huì)把機(jī)動(dòng)加速度當(dāng)成重力方向修正導(dǎo)致姿態(tài)被拉偏。解決根據(jù)加速度計(jì)模長(zhǎng)和陀螺儀角速度做自適應(yīng)。如果 |acc| 偏離 9.81 超過 10%或者角速度超過閾值臨時(shí)調(diào)大 R_accel 或跳過更新。我一般設(shè)一個(gè)門限|acc_norm - 1| 0.1 時(shí)R_accel 乘以 100。4.3 現(xiàn)象陀螺儀偏差估計(jì)收斂極慢幾分鐘還在漂原因Q_bias 設(shè)得太小濾波器認(rèn)為偏差是常數(shù)不敢更新。或者觀測(cè)方程對(duì)偏差的雅可比接近零偏差不可觀。解決調(diào)大 Q_bias給偏差留出變化空間。如果偏差不可觀比如只有加速度計(jì)觀測(cè)繞重力軸旋轉(zhuǎn)的偏差不可測(cè)需要加磁力計(jì)或者零速修正。我一般會(huì)在靜止時(shí)強(qiáng)制做一次偏差校準(zhǔn)把估計(jì)值直接寫回。4.4 現(xiàn)象新息序列出現(xiàn)連續(xù)同號(hào)濾波器明顯滯后原因Q 太小模型太“自信”觀測(cè)信息被壓制?；蛘?H 矩陣算錯(cuò)了觀測(cè)方向?qū)Σ簧稀＝鉀Q先檢查 H 的符號(hào)和坐標(biāo)系。如果 H 沒問題調(diào)大 Q_quat。可以用新息的自相關(guān)函數(shù)判斷如果滯后 1 步的自相關(guān)超過 0.5說明 Q 偏小。我一般把 Q_quat 乘以 10 試一次看新息是否變白。4.5 現(xiàn)象嵌入式上跑幾分鐘后輸出卡死或亂碼原因單精度浮點(diǎn)下四元數(shù)歸一化時(shí)除以接近零的模長(zhǎng)產(chǎn)生 NaN?；蛘呔仃嚽竽鏁r(shí)行列式下溢。解決歸一化前檢查模長(zhǎng)小于 1e-6 時(shí)重置四元數(shù)。矩陣求逆加 det 保護(hù)如第 3 章代碼所示。另外所有除法都要檢查分母。我習(xí)慣在每步更新后加一個(gè) assert檢查狀態(tài)量是否在合理范圍一旦越界就重置濾波器。5. 進(jìn)階技巧用新息白噪聲檢驗(yàn)判斷濾波器是否“健康”EKF 調(diào)參最怕盲目試湊。我后來養(yǎng)成一個(gè)習(xí)慣每次跑完數(shù)據(jù)先把新息序列 y_k z_k - h(x?_k?) 拿出來做白噪聲檢驗(yàn)。如果 y_k 是零均值白噪聲說明濾波器工作正常如果出現(xiàn)有色噪聲或均值偏移說明 Q、R 或模型有問題。具體做法計(jì)算新息的均值和自相關(guān)。均值應(yīng)該接近零自相關(guān)在滯后 1 步后應(yīng)該迅速衰減。如果自相關(guān)在滯后 1 步超過 0.5說明 Q 偏小如果均值明顯偏離零說明觀測(cè)模型有系統(tǒng)偏差。def innovation_test(innovations): # innovations: N x m 矩陣每行是一個(gè)時(shí)刻的新息 N, m innovations.shape mean np.mean(innovations, axis0) # 自相關(guān)滯后 1 步 autocorr [] for i in range(m): y innovations[:, i] - mean[i] r1 np.sum(y[1:] * y[:-1]) / np.sum(y * y) autocorr.append(r1) return mean, autocorr # 示例輸出 # mean [0.001, -0.002, 0.0005] 接近零 # autocorr [0.12, 0.08, 0.15] 小于 0.5通過這個(gè)檢驗(yàn)幫我省了很多瞎調(diào)的時(shí)間。有一次姿態(tài)估計(jì)滯后新息自相關(guān)到 0.7我把 Q_quat 調(diào)大 5 倍后降到 0.2姿態(tài)響應(yīng)立刻跟上了。從那以后我每次改完 Q、R都強(qiáng)制走一遍新息檢驗(yàn)不通過就不上機(jī)。還有一個(gè)進(jìn)階用法自適應(yīng) EKF。根據(jù)新息的統(tǒng)計(jì)特性在線調(diào)整 R。如果新息方差突然變大說明觀測(cè)噪聲增加了臨時(shí)調(diào)大 R。這個(gè)在 GPS 信號(hào)遮擋場(chǎng)景特別有用但實(shí)現(xiàn)時(shí)要加限幅避免 R 被調(diào)到無窮大。希望這些經(jīng)驗(yàn)幫到你少走點(diǎn)我當(dāng)年走過的彎路。本文還有配套的精品資源點(diǎn)擊獲取