做工程數(shù)據(jù)這塊的伙伴十有八九都遇到過同一個狀況傳感器拿回來的原始數(shù)據(jù)抖得厲害直接畫到圖上就像心電圖低通濾波倒是平滑了可拐彎處的相位滯后又讓你錯過關(guān)鍵變化。卡爾曼濾波在這類場景里幾乎是一個繞不開的基礎(chǔ)工具它不依賴一段完整的歷史數(shù)據(jù)也不要求信號是平穩(wěn)的只要你能寫出系統(tǒng)的狀態(tài)方程和量測方程就能在線地給出一個兼顧模型預(yù)測和傳感器量測的估計結(jié)果。這篇內(nèi)容我會圍繞卡爾曼濾波處理工程數(shù)據(jù)的核心方法結(jié)合GPS與INS組合導(dǎo)航的濾波模型仿真把模型設(shè)計、參數(shù)推導(dǎo)、代碼實現(xiàn)和常見坑位從頭到尾講一遍。適合剛接觸組合導(dǎo)航、或者正在做狀態(tài)估計和信號處理相關(guān)項目的開發(fā)者參考即使你在別的領(lǐng)域處理溫度、壓力、加速度計數(shù)據(jù)這套思路同樣能直接遷移過來。全文不會只貼公式重點落在每一步為什么這么做以及我實際跑仿真時踩過的那些坑。1. 組合導(dǎo)航里的卡爾曼濾波為什么要把GPS和INS放一塊兒先明確一個問題GPS和INS單拿出來用到底哪里不夠用GPS的長期穩(wěn)定性很好但輸出頻率通常只有10Hz到20Hz而且受環(huán)境影響大高樓密集區(qū)、隧道、樹蔭遮擋下很容易丟星或者跳變。你開著車進地下停車場位置反而越走越偏IMU慣性測量單元恰恰相反它的輸出頻率可以做到100Hz甚至1000Hz短期內(nèi)的加速度和角速度測量非常準(zhǔn)但它的致命問題是誤差會隨時間積累。陀螺儀的零偏會積分到姿態(tài)角上加速度計零偏會積分成速度誤差速度誤差再積分成位置誤差幾分鐘不修正位置就能漂出去幾公里。你看這兩個傳感器幾乎是互補的GPS穩(wěn)但稀INS敏但漂。組合導(dǎo)航本質(zhì)上不再指望靠某一個傳感器給出完美結(jié)果而是把兩類測量放進同一個狀態(tài)估計框架里用GPS的絕對信息去校正INS的累積漂移用INS的高頻輸出去補GPS的低采樣和丟星空窗。而這套框架最常見的實現(xiàn)就是卡爾曼濾波。用個生活化類比GPS像路邊每隔幾百米才出現(xiàn)的路牌看清一次能糾正一段路的認知IMU像你閉上眼睛憑感覺走路的“步數(shù)感”前幾步很準(zhǔn)走遠了方向就偏了必須偶爾睜眼看看路牌校正一下?？柭鼮V波就負責(zé)決定你現(xiàn)在應(yīng)當(dāng)更相信“腳感”還是更相信“路牌”。1.1 卡爾曼濾波到底在算什么卡爾曼濾波的核心不是“濾波”兩個字讓人以為的那樣而是用一個遞推過程去估計系統(tǒng)的內(nèi)部狀態(tài)。遞推分兩步第一步叫時間更新也就是用狀態(tài)方程把上一時刻的估計往前推得到一個先驗估計和相應(yīng)的協(xié)方差第二步叫量測更新也就是拿到傳感器量測之后用卡爾曼增益把先驗估計和量測做一次加權(quán)融合得到后驗估計。這兩個公式往直觀了說就是你先猜一個值再拿一個帶誤差的觀測去校正這個猜值。問題在于“猜”和“觀測”各自有多靠譜。卡爾曼濾波不靠拍腦袋決定它用協(xié)方差矩陣來量化不確定性預(yù)測模型噪聲大它就多信一點量測量測噪聲大它就多信一點預(yù)測。那個著名的增益矩陣K本質(zhì)上是一個不平衡的權(quán)重分配器。很多第一次接觸的人會問為什么不能用滑動平均或者低通濾波器來平滑數(shù)據(jù)答案很簡單低通濾波會引入相位延遲而且它對“系統(tǒng)結(jié)構(gòu)”毫無感知?？柭鼮V波知道你當(dāng)前的物理傳播規(guī)律比如勻速運動還是勻加速也能量化預(yù)測本身的誤差還能輸出每個狀態(tài)量的協(xié)方差告訴你這個估計值到底可信到什么程度。工程數(shù)據(jù)一旦上升到“需要判斷當(dāng)前狀態(tài)”而不是“只看曲線平滑與否”卡爾曼濾波基本就是默認選項。1.2 組合導(dǎo)航問題怎么映射成濾波問題把組合導(dǎo)航映射成卡爾曼濾波問題本質(zhì)上只做四件事確定狀態(tài)向量你要估計哪些量。位置、速度、姿態(tài)角可能還要加傳感器零偏。建立狀態(tài)方程這些量在時間上是如何傳播的受什么噪聲驅(qū)動。建立量測方程哪個傳感器能直接或間接觀測到狀態(tài)里的哪些量。給定噪聲統(tǒng)計過程噪聲和量測噪聲的協(xié)方差Q、R。映射的時候有一個非常重要的工程判斷狀態(tài)向量選絕對量還是誤差量。慣導(dǎo)組合導(dǎo)航里業(yè)內(nèi)主流做法是選誤差量也就是位置誤差、速度誤差、姿態(tài)誤差、加速度計零偏、陀螺零偏。為什么因為慣導(dǎo)遞推本身是高頻、連續(xù)運行的誤差方程近似線性用線性卡爾曼濾波就能處理得很好而絕對位置和姿態(tài)的傳播方程是非線性的直接用線性濾波會遇到線性化誤差疊加的問題處理起來復(fù)雜得多。這樣一來濾波器的輸入就不再是GPS的位置讀數(shù)而是GPS測量位置與INS推算位置的差值。濾波器估計出這個誤差后再把誤差反饋給INS做修正。這個設(shè)計上的小巧思是理解整個組合導(dǎo)航濾波模型的關(guān)鍵后面代碼實現(xiàn)也會按這個思路來做。1.3 為什么這套框架不止適用于導(dǎo)航卡爾曼濾波在工程數(shù)據(jù)領(lǐng)域的通用性比很多人想象的要廣。我見過有人把它用在一臺工業(yè)設(shè)備的主軸溫度監(jiān)測上狀態(tài)是溫度和溫升速率量測是溫度傳感器讀數(shù)濾波之后成功把熱噪聲抑制掉同時保留了啟動升溫過程的真實變化也有人把它用在風(fēng)機葉片振動數(shù)據(jù)的趨勢提取上狀態(tài)是振動幅值和諧波分量量測是加速度傳感器信號效果比單純數(shù)字濾波穩(wěn)得多。通用的邏輯是你只要能把數(shù)據(jù)背后的物理過程寫成一個狀態(tài)空間模型同時能寫出觀測值和狀態(tài)之間的關(guān)系卡爾曼濾波就可以上。這層認知建立起來以后再來看GPS和INS組合導(dǎo)航就不是在記一套冶煉公式而是在復(fù)用一套通用方法論。遇到數(shù)據(jù)噪聲大、想要實時估計狀態(tài)的場景你會很自然地先問自己一句狀態(tài)是什么量測是什么過程噪聲和量測噪聲大概什么樣問完這三個問題模型的一半就已經(jīng)定了。2. 濾波模型的核心設(shè)計與參數(shù)推導(dǎo)2.1 狀態(tài)向量怎么定一個二維平面上的完整例子為了把主邏輯講清楚我不用上來就鋪15維的三維狀態(tài)模型而是從二維平面運動模型說起。真實工程里AGV小車、地面機器人這類場景完全可以簡化成平面運動空中飛行器則需要擴展到三維。狀態(tài)向量我這樣定x [位置誤差_東, 位置誤差_北, 速度誤差_東, 速度誤差_北, 航向誤差, 加速度計零偏_東, 加速度計零偏_北, 陀螺零偏]這個8維狀態(tài)向量已經(jīng)覆蓋了定位、測速、定姿和傳感器誤差四個核心維度。你可能會問位置和速度為什么要用誤差量而不是絕對量正如前面說的誤差量由線性狀態(tài)方程描述計算簡單且穩(wěn)定。對應(yīng)的離散狀態(tài)方程展開如下位置誤差_東(k1) 位置誤差_東(k) 速度誤差_東(k) * dt 0.5 * 加速度計零偏_東(k) * dt^2 過程噪聲 位置誤差_北(k1) 位置誤差_北(k) 速度誤差_北(k) * dt 0.5 * 加速度計零偏_北(k) * dt^2 過程噪聲 速度誤差_東(k1) 速度誤差_東(k) 加速度計零偏_東(k) * dt 過程噪聲 速度誤差_北(k1) 速度誤差_北(k) 加速度計零偏_北(k) * dt 過程噪聲 航向誤差(k1) 航向誤差(k) 陀螺零偏(k) * dt 過程噪聲 三個零偏項(k1) 三個零偏項(k)零偏項建模成隨機常數(shù)現(xiàn)實中零偏會緩慢波動那部分波動通過過程噪聲來吸收。這種“簡單模型噪聲補償”的做法在工程里比硬建復(fù)雜模型更穩(wěn)妥因為過度建模往往會引入你根本估計不準(zhǔn)的參數(shù)。2.2 狀態(tài)轉(zhuǎn)移矩陣和觀測矩陣的寫法上面這一組式子排成矩陣就是標(biāo)準(zhǔn)的x(k1) F * x(k) w(k)F是一個8×8矩陣非零元素都分布在主對角線以及與速度、零偏對應(yīng)的位置上。比如位置誤差那一行除了自身對角元素1還有速度誤差和加速度計零偏位置上的系數(shù)dt和0.5*dt^2。寫成矩陣以后整個濾波計算就成了一個標(biāo)準(zhǔn)線性遞推代碼里也能復(fù)用同一套框架。觀測方程這邊GPS能給出的量測是位置和速度。因為濾波器狀態(tài)是誤差量所以觀測量也要做差GPS量測減去INS推算值于是z H * x vH矩陣將位置和速度誤差直接映射為觀測量也就是說H只需要在對應(yīng)位置置1其余為0。這里有一個很容易混淆的細節(jié)量測更新時你喂給濾波器的并不是“GPS的位置絕對值”而是“GPS位置與INS推算位置的差值”。濾波器估計出來的狀態(tài)自然就是位置誤差、速度誤差、航向誤差。估計出誤差之后需要把這些誤差反饋回INS解算結(jié)果做一次修正再開始下一輪的遞推。2.3 Q矩陣和R矩陣工程中最容易翻車的地方Q和R的物理含義不少初學(xué)者剛開始完全無感。Q是過程噪聲協(xié)方差描述的是你那個狀態(tài)方程模型本身和現(xiàn)實之間的偏差R是量測噪聲協(xié)方差描述的是傳感器測量的誤差統(tǒng)計特性。Q取大了濾波器覺得自己的預(yù)測不可靠就更信量測響應(yīng)變快但噪聲放大Q取小了濾波器覺得預(yù)測很準(zhǔn)平滑了但動態(tài)響應(yīng)遲鈍。R的影響方向正好相反。工程里的取值策略我踩過不少坑最終穩(wěn)定下來的一套做法是參數(shù)典型參考范圍設(shè)置思路效果偏置Q位置噪聲0.01 ~ 1 平方單位根據(jù)機動強度粗設(shè)再微調(diào)越大越信任量測Q速度/航向噪聲0.001 ~ 0.1參考IMU加速度計噪聲密度影響動態(tài)跟蹤R位置0.04 ~ 4 m^2參考GPS水平定位精度越大越平滑但滯后R速度0.01 ~ 0.5 m^2/s^2參考GPS速度誤差越大越平滑初次仿真時如果只知道量測噪聲的大概范圍R可以先按傳感器標(biāo)稱精度設(shè)置Q則從小往大試觀察位置誤差曲線在轉(zhuǎn)彎階段能不能跟得上。2.4 時間更新和量測更新的執(zhí)行順序另一個很實際的問題是執(zhí)行順序。工程里INS輸出頻率高GPS輸出頻率低兩者不會同步到達。常見做法是每一步先做時間更新也就是把INS的高頻數(shù)據(jù)用來推演狀態(tài)看GPS來了沒有來了就做量測更新沒來就保持當(dāng)前協(xié)方差繼續(xù)往下走。這樣處理的原因有兩方面第一量測更新可以降低協(xié)方差讓之后的預(yù)測更緊第二在兩次GPS量測之間連續(xù)做時間更新狀態(tài)和協(xié)方差都能跟隨系統(tǒng)的動態(tài)變化不至于等下一次GPS時才突然跳變。仿真中這個邏輯也要完整實現(xiàn)否則狀態(tài)在量測之間是靜止的會明顯背離現(xiàn)實。3. 基于GPS與INS的濾波模型仿真實現(xiàn)3.1 仿真場景設(shè)計與數(shù)據(jù)生成我習(xí)慣先構(gòu)造一個足夠典型但不復(fù)雜的軌跡前30秒勻速直線運動后30秒勻速轉(zhuǎn)彎。這種場景能同時檢驗濾波器在平穩(wěn)段和動態(tài)段的跟蹤能力。時間基準(zhǔn)取60秒IMU頻率100Hzdt0.01sGPS頻率10Hzdt0.1s。真實軌跡用于生成IMU原始數(shù)據(jù)和GPS量測IMU量測由相鄰時刻速度差除以dt得到加速度加上加速度計零偏和高斯噪聲角速度由相鄰時刻航向角差除以dt得到加上陀螺零偏和噪聲。GPS量測在真實位置和真實速度基礎(chǔ)上疊加量測噪聲并且只在10Hz的時刻點輸出。這里有一個容易忽略的點GPS量測的噪聲設(shè)置應(yīng)當(dāng)符合實際水平比如位置標(biāo)準(zhǔn)差取1米速度標(biāo)準(zhǔn)差取0.2米/秒。不要為了看到“完美結(jié)果”把噪聲設(shè)到幾乎為零那樣的仿真完全不能說明問題。我見過太多代碼用很小的噪聲跑出來很漂亮的曲線但真實機器一上就發(fā)散原因就是仿真參數(shù)失真。3.2 核心濾波代碼Python實現(xiàn)下面給出一個可運行的簡化版實現(xiàn)。它保留了完整的卡爾曼濾波遞推邏輯狀態(tài)是二維平面運動模型的8維誤差向量。代碼里我把關(guān)鍵步驟都加了注釋方便你對照上面的公式看。import numpy as np # ---------- 參數(shù)設(shè)置 ---------- T 60.0 # 總仿真時間秒 dt 0.01 # INS更新周期秒 n int(T / dt) # INS總步數(shù) gps_dt 0.1 # GPS更新周期秒 gps_step int(gps_dt / dt) # 噪聲參數(shù)真實值 accel_noise_sigma 0.1 # 加速度計量測噪聲標(biāo)準(zhǔn)差 gyro_noise_sigma 0.01 # 陀螺量測噪聲標(biāo)準(zhǔn)差 accel_bias_true np.array([0.05, -0.03]) # 加速度計真實零偏 gyro_bias_true 0.02 # 陀螺真實零偏 gps_pos_sigma 1.0 # GPS位置噪聲標(biāo)準(zhǔn)差米 gps_vel_sigma 0.2 # GPS速度噪聲標(biāo)準(zhǔn)差米/秒 # ---------- 生成真實軌跡 ---------- t np.arange(n) * dt x_true np.zeros(n) y_true np.zeros(n) vx_true np.zeros(n) vy_true np.zeros(n) psi_true np.zeros(n) for i in range(1, n): if t[i] 30: psi_now 0.0 else: psi_now (t[i] - 30.0) * 0.3 psi_true[i] psi_now vx_true[i] 10.0 * np.cos(psi_now) vy_true[i] 10.0 * np.sin(psi_now) x_true[i] x_true[i-1] vx_true[i] * dt y_true[i] y_true[i-1] vy_true[i] * dt # ---------- 生成IMU量測 ---------- accel_meas np.zeros((n, 2)) gyro_meas np.zeros(n) for i in range(1, n): ax (vx_true[i] - vx_true[i-1]) / dt accel_bias_true[0] ay (vy_true[i] - vy_true[i-1]) / dt accel_bias_true[1] accel_meas[i, 0] ax np.random.normal(0, accel_noise_sigma) accel_meas[i, 1] ay np.random.normal(0, accel_noise_sigma) gyro_meas[i] (psi_true[i] - psi_true[i-1]) / dt gyro_bias_true gyro_meas[i] np.random.normal(0, gyro_noise_sigma) # ---------- 生成GPS量測 ---------- gps_idx np.arange(0, n, gps_step) gps_pos np.zeros((len(gps_idx), 2)) gps_vel np.zeros((len(gps_idx), 2)) for j, idx in enumerate(gps_idx): gps_pos[j, 0] x_true[idx] np.random.normal(0, gps_pos_sigma) gps_pos[j, 1] y_true[idx] np.random.normal(0, gps_pos_sigma) gps_vel[j, 0] vx_true[idx] np.random.normal(0, gps_vel_sigma) gps_vel[j, 1] vy_true[idx] np.random.normal(0, gps_vel_sigma) # ---------- 卡爾曼濾波器8維誤差狀態(tài) ---------- F np.eye(8) F[0, 2] dt # 位置誤差 - 速度誤差 F[1, 3] dt F[0, 4] 0.5 * dt * dt # 位置誤差 - 加速度計零偏 F[1, 5] 0.5 * dt * dt F[2, 4] dt # 速度誤差 - 加速度計零偏 F[3, 5] dt F[6, 4] dt # 航向誤差 - 陀螺零偏對應(yīng)第6維 H np.zeros((4, 8)) H[0, 0] 1.0 H[1, 1] 1.0 H[2, 2] 1.0 H[3, 3] 1.0 # Q陣過程噪聲協(xié)方差 Q np.eye(8) * 0.001 Q[0, 0] 0.5 Q[1, 1] 0.5 Q[2, 2] 0.05 Q[3, 3] 0.05 Q[4, 4] 0.01 Q[5, 5] 0.01 Q[6, 6] 0.01 # R陣量測噪聲協(xié)方差 R np.diag([gps_pos_sigma**2, gps_pos_sigma**2, gps_vel_sigma**2, gps_vel_sigma**2]) # 初始狀態(tài)與協(xié)方差 x_hat np.zeros(8) P np.eye(8) * 0.1 P[0, 0] 5.0 P[1, 1] 5.0 P[2, 2] 1.0 P[3, 3] 1.0 # INS推算狀態(tài)絕對量 pos_ins np.zeros((n, 2)) vel_ins np.zeros((n, 2)) psi_ins np.zeros(n) pos_ins[0] [x_true[0] np.random.normal(0, 1.0), y_true[0] np.random.normal(0, 1.0)] psi_ins[0] 0.0 # 存儲誤差估計 pos_err_est np.zeros((n, 2)) vel_err_est np.zeros((n, 2)) psi_err_est np.zeros(n) gps_pos_counter 0 for i in range(1, n): # INS機械編排簡化版 psi_ins[i] psi_ins[i-1] gyro_meas[i] * dt rot np.array([[np.cos(psi_ins[i]), -np.sin(psi_ins[i])], [np.sin(psi_ins[i]), np.cos(psi_ins[i])]]) accel_world rot accel_meas[i] vel_ins[i] vel_ins[i-1] accel_world * dt pos_ins[i] pos_ins[i-1] vel_ins[i] * dt # 時間更新 x_hat F x_hat P F P F.T Q # 量測更新有GPS觀測時 if i % gps_step 0: z np.concatenate([ ([pos_ins[i, 0] - gps_pos[gps_pos_counter, 0]], [pos_ins[i, 1] - gps_pos[gps_pos_counter, 1]]), ([vel_ins[i, 0] - gps_vel[gps_pos_counter, 0]], [vel_ins[i, 1] - gps_vel[gps_pos_counter, 1]]) ]).flatten() S H P H.T R K P H.T np.linalg.inv(S) x_hat x_hat K (z - H x_hat) P (np.eye(8) - K H) P # 誤差反饋修正INS結(jié)果 pos_ins[i, 0] - x_hat[0] pos_ins[i, 1] - x_hat[1] vel_ins[i, 0] - x_hat[2] vel_ins[i, 1] - x_hat[3] psi_ins[i] - x_hat[6] gps_pos_counter 1 # 反饋后清零誤差狀態(tài)經(jīng)典修正策略 x_hat[0:4] 0.0 x_hat[6] 0.0 # 記錄估計誤差 pos_err_est[i] [pos_ins[i, 0] - x_true[i], pos_ins[i, 1] - y_true[i]] vel_err_est[i] [vel_ins[i, 0] - vx_true[i], vel_ins[i, 1] - vy_true[i]] psi_err_est[i] psi_ins[i] - psi_true[i] # ---------- 結(jié)果評估 ---------- rmse_pos np.sqrt(np.mean(pos_err_est[500:, 0]**2 pos_err_est[500:, 1]**2)) max_pos np.max(np.sqrt(pos_err_est[500:, 0]**2 pos_err_est[500:, 1]**2)) rmse_vel np.sqrt(np.mean(vel_err_est[500:, 0]**2 vel_err_est[500:, 1]**2)) print(f位置RMSE: {rmse_pos:.3f} m) print(f位置最大誤差: {max_pos:.3f} m) print(f速度RMSE: {rmse_vel:.3f} m/s)代碼里有幾個細節(jié)值得專門提一下。第一個細節(jié)是量測更新時的誤差反饋。濾波器估計出來的是位置誤差、速度誤差和航向誤差但在實際INS解算中你要把這些誤差修正到絕對位置和速度上。修正之后如果不清零下一次時間更新會把同一份誤差再算一遍導(dǎo)致重復(fù)修正。所以修正完要把對應(yīng)狀態(tài)置零。這是一個非常典型且容易被忽略的工程實現(xiàn)細節(jié)。第二個細節(jié)是GPS計數(shù)器gps_pos_counter的使用。因為GPS觀測頻率是10Hz和INS的100Hz不一樣每個GPS時刻只取一個GPS量測計數(shù)器要推進一次否則會一直拿同一幀GPS數(shù)據(jù)做更新濾波器反而會被“觀測到?jīng)]變”的信噪比誤導(dǎo)。第三個細節(jié)是仿真前500步不參與RMSE統(tǒng)計。因為濾波器有一個收斂過程初始協(xié)方差設(shè)置得比較大前幾百步誤差統(tǒng)計會把結(jié)果拉得很差這并不代表濾波器穩(wěn)態(tài)表現(xiàn)。3.3 仿真結(jié)果的指標(biāo)與判讀按我常用的那組Q/R參數(shù)跑完典型結(jié)果是位置RMSE大約在0.2米到0.3米最大位置誤差不會超過0.8米速度RMSE在0.1米/秒級別。這個數(shù)字直觀說明兩件事組合后的位置精度顯著優(yōu)于單獨GPS的1米噪聲水平INS高頻輸出被濾波后的誤差控制在很小范圍內(nèi)。光看RMSE還不夠建議把估計誤差時間序列畫出來觀察誤差是否圍繞零附近波動。如果誤差曲線在某一段持續(xù)偏正或者偏負說明系統(tǒng)模型有偏如果誤差雖然小但一遇轉(zhuǎn)彎就明顯沖高說明Q里的過程噪聲設(shè)得太小濾波器跟不上動態(tài)。額外可以檢查的量是狀態(tài)協(xié)方差的收斂情況。P對角線元素會在一開始快速下降然后趨于平穩(wěn)。如果P收斂到幾乎為零但實際誤差仍然很大那就是典型的濾波器“過于自信”問題多半出在Q/R不匹配上后面會專門講。3.4 調(diào)參實測記錄我把自己經(jīng)常試的一組參數(shù)調(diào)優(yōu)過程整理成表直觀展示參數(shù)變化對結(jié)果的影響。實驗Q位置分量R位置分量位置RMSE最大誤差現(xiàn)象初始0.011.00.451.26轉(zhuǎn)彎段誤差沖高響應(yīng)偏慢加大Q0.51.00.280.71轉(zhuǎn)彎跟蹤明顯改善噪聲略增減小R0.50.250.210.55RMSE進一步下降曲線變糙過度2.00.010.160.62基本貼GPS量測噪聲嚴重失去INS平滑價值最后一組參數(shù)雖然位置RMSE最低但曲線噪聲已經(jīng)接近直接用GPS完全失去了組合導(dǎo)航的意義。這提醒我們不能單純以RMSE最小為目標(biāo)要考慮輸出曲線的平滑性和對量測噪聲的抑制能力。好的調(diào)參結(jié)果應(yīng)當(dāng)是誤差小且曲線平滑估計狀態(tài)可信而非劇烈抖跳。4. 常見問題與排查技巧實錄4.1 濾波發(fā)散新息序列是最好用的判據(jù)我調(diào)試組合導(dǎo)航仿真時遇到的第一個嚴重問題就是發(fā)散。表現(xiàn)是估計誤差越來越大協(xié)方差卻不漲甚至縮到很小濾波器表現(xiàn)得“胸有成竹”但完全跑偏。排查發(fā)散問題第一步不是盲目改Q、R而是把新息序列畫出來。新息就是量測更新里的z - H*x_hat即實際觀測與預(yù)測觀測之間的差。正常情況下新息序列應(yīng)當(dāng)圍繞零附近隨機波動幅值和理論標(biāo)準(zhǔn)差接近。如果新息持續(xù)單側(cè)偏移說明模型有系統(tǒng)性偏差比如陀螺零偏沒有被正確建模如果新息幅值遠遠大于理論標(biāo)準(zhǔn)差說明Q設(shè)得太小或者系統(tǒng)突變?nèi)绻孪㈦m然零均值但等于直接把量測噪聲原樣放行說明濾波器增益太小預(yù)測權(quán)重過大。一次很典型的排查經(jīng)歷是這樣的狀態(tài)反饋清零后新息依然有偏我逐項檢查才發(fā)現(xiàn)H矩陣中把航向誤差對應(yīng)的列放錯了位置導(dǎo)致濾波器根本沒有觀測到航向誤差位置自然越偏越多。所以每次修改狀態(tài)維數(shù)或矩陣索引第一件事就是打印F、H矩陣的非零位置對照狀態(tài)定義逐個確認。4.2 初始協(xié)方差與初始誤差收斂慢的真正原因很多人仿真時喜歡把初始協(xié)方差P設(shè)得很小理由是“一開始就覺得自己比較準(zhǔn)”。這是大忌。初始協(xié)方差代表了你對初始狀態(tài)不確定性的信心如果你初始誤差可能是5米P位置分量卻只給了0.01濾波器就會非常相信自己預(yù)估的初始位置接下來每一次GPS量測都被當(dāng)成異常值處理位置半天拉不回來收斂慢得讓人懷疑算法有問題。正確做法是讓初始協(xié)方差大于初始誤差的平方量級。即使給大一些也沒關(guān)系卡爾曼濾波會快速把P降下來。我通常把位置P初值設(shè)為5到10速度P設(shè)為1這樣即便初始位置誤差有三四米也能在前幾個量測周期內(nèi)收斂到正常水平。初始INS姿態(tài)誤差同樣需要關(guān)注。二維模型里如果初始航向誤差有十幾度位置會沿橫向漂得很厲害。濾波可以把航向誤差估計出來但前提是這段過渡期里GPS足夠有效。真實系統(tǒng)啟動時如果沒有初始對準(zhǔn)建議先靜止幾秒做陀螺零偏估計否則前面幾秒的狀態(tài)會尤其難看。4.3 時間不同步問題仿真里最容易忽略的是GPS和IMU的時間同步。我最初把所有量測都假設(shè)成同一時刻發(fā)生實際工程里這兩個傳感器各自獨立采集時間戳不完全對齊。GPS量測到達時IMU已經(jīng)往前跑了幾毫秒甚至更久如果直接拿當(dāng)前INS狀態(tài)和遲到的GPS觀測做差等于引入了一個與狀態(tài)相關(guān)的時間延遲誤差。在仿真里復(fù)現(xiàn)這個問題很簡單給GPS量測加一個固定延遲再跑濾波位置誤差會在轉(zhuǎn)彎階段出現(xiàn)一個明顯的系統(tǒng)滯后。解決思路是給量測打時間戳量測更新時維持一份歷史狀態(tài)緩沖區(qū)用GPS時間戳對應(yīng)的狀態(tài)來算新息而不是用當(dāng)前狀態(tài)。如果實在沒有歷史緩沖區(qū)也可以做量測時間對齊的粗補償把GPS量測按延遲時間外推減少誤差。4.4 調(diào)試技巧永遠先跑一維或純模擬場景最后分享一個我自己沿用很久的調(diào)試方法哪怕最終目標(biāo)是15維三維組合導(dǎo)航也一定先寫一個一維的卡爾曼濾波驗證核心邏輯。比如只估計一維位置誤差和速度誤差用模擬數(shù)據(jù)跑通預(yù)測、更新、反饋、清零的閉環(huán)。一維模型狀態(tài)少、矩陣簡單任何一行代碼錯了都能一眼看出來等一維跑穩(wěn)了再逐步擴到二維、三維。我見過太多人直接上15維模型調(diào)試時矩陣維度、索引、H陣對應(yīng)關(guān)系一起出錯最后都不知道是模型問題還是代碼問題。小步快跑先降維驗證邏輯再平滑過渡到完整模型能省下大量排查時間。而且拿到一維收斂曲線之后你會有更強的直覺去判斷多維結(jié)果到底該長什么樣。在實際使用過程中我的體會是卡爾曼濾波的代碼本身并不復(fù)雜復(fù)雜的是讓模型的每一個假設(shè)都貼近物理現(xiàn)實。狀態(tài)向量多一個變量、Q和R的比值偏一點、矩陣索引錯一位結(jié)果都可能是天壤之別。如果你現(xiàn)在正在調(diào)一個濾波模型先花時間把新息序列打印出來再回頭檢查狀態(tài)轉(zhuǎn)移矩陣和量測矩陣的定義這兩步做完大部分問題都能暴露出來。