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