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