開車時(shí)手機(jī)導(dǎo)航上的定位點(diǎn)是不是經(jīng)常在路口亂跳這不是手機(jī)壞了而是GPS信號本身帶噪聲在城區(qū)高樓之間誤差幾米到十幾米都很常見。卡爾曼濾波這種經(jīng)典的狀態(tài)估計(jì)算法干的就是從這些帶噪聲的測量里猜出真實(shí)狀態(tài)的活。1960年魯?shù)婪颉た柭岢鏊臅r(shí)候大概也沒想到半個(gè)多世紀(jì)后它依然是自動(dòng)駕駛、機(jī)器人、航空航天領(lǐng)域最常用的傳感器融合算法。這篇文章我不整虛的直接帶你從原理直覺一路走到Python代碼實(shí)現(xiàn)把五個(gè)核心公式、完整可跑的代碼、調(diào)參經(jīng)驗(yàn)和發(fā)散避坑一起說透。本文適合這幾類人剛接觸卡爾曼濾波的在校學(xué)生、在機(jī)器人和無人機(jī)項(xiàng)目里被噪聲折磨的工程師、以及任何想在代碼里用最快速度跑通一個(gè)濾波器的人。我會(huì)盡量用大白話和具體數(shù)字講原理保證你跟著敲完代碼就能理解每一行在干什么。1. 卡爾曼濾波到底在解決什么問題先說一個(gè)最樸素的問題你有一個(gè)傳感器讀數(shù)不準(zhǔn)怎么估計(jì)真實(shí)值最笨的辦法是多測幾次取平均。假設(shè)你用一個(gè)溫度計(jì)測恒溫箱溫度每次讀數(shù)都不一樣取10次的平均值確實(shí)比單次讀數(shù)穩(wěn)。但這個(gè)辦法有個(gè)明顯的缺點(diǎn)如果溫度本身在緩慢變化你取平均得到的是過去一段時(shí)間的平均溫度而不是當(dāng)前溫度滑窗越大滯后越嚴(yán)重。而且你沒有利用溫度隨時(shí)間的變化規(guī)律這個(gè)信息——實(shí)際上我們通常知道這個(gè)規(guī)律比如熱慣量導(dǎo)致溫度不可能瞬間跳變?？柭鼮V波的思路完全不一樣它同時(shí)使用兩個(gè)信息來源一個(gè)是系統(tǒng)模型對下一時(shí)刻狀態(tài)的預(yù)測另一個(gè)是傳感器對當(dāng)前狀態(tài)的觀測然后按照兩者的可信度動(dòng)態(tài)加權(quán)。這個(gè)可信度不是拍腦袋定的而是隨著每次遞歸更新自動(dòng)調(diào)整的。這就是它和滑動(dòng)平均、低通濾波器最本質(zhì)的區(qū)別。1.1 用溫度測量的例子說清濾波的本質(zhì)假設(shè)恒溫箱的真實(shí)溫度是23.0°C溫度計(jì)的觀測噪聲方差是1.0也就是標(biāo)準(zhǔn)差1°C。單次讀數(shù)可能在22到24之間浮動(dòng)。如果用卡爾曼濾波我們首先建立一個(gè)非常簡單的系統(tǒng)模型下一時(shí)刻的溫度 ≈ 當(dāng)前溫度 過程噪聲。過程噪聲代表你沒有建模的因素比如開關(guān)門帶進(jìn)來的氣流、加熱器自身的波動(dòng)假設(shè)它方差很小比如0.01。這樣就有了兩個(gè)不完美的信息來源模型預(yù)測基于上一時(shí)刻估計(jì)值外推好處是穩(wěn)定壞處是沒有觀測修正會(huì)越來越偏。傳感器觀測直接反映當(dāng)前狀態(tài)好處是真實(shí)壞處是每個(gè)點(diǎn)都帶噪聲?？柭鼮V波每一輪做的事情就一句話用模型預(yù)測得到先驗(yàn)估計(jì)再用觀測修正得到后驗(yàn)估計(jì)。至于修正多少由一個(gè)叫卡爾曼增益的系數(shù)決定。這個(gè)增益不是固定的而是每一幀動(dòng)態(tài)算出來的。1.2 為什么滑動(dòng)平均和低通濾波不是最優(yōu)解拿滑動(dòng)平均來說它隱含的假設(shè)是所有歷史數(shù)據(jù)的權(quán)重相等這在系統(tǒng)狀態(tài)穩(wěn)定時(shí)沒問題但一旦狀態(tài)發(fā)生變化比如目標(biāo)突然加速滑動(dòng)平均會(huì)反應(yīng)遲鈍因?yàn)榕f數(shù)據(jù)還在起著同樣的權(quán)重。低通濾波雖然可以做到指數(shù)衰減權(quán)重但系數(shù)的選擇基本靠經(jīng)驗(yàn)和試錯(cuò)而且無法顯式處理我模型預(yù)測得準(zhǔn)不準(zhǔn)傳感器這陣子可靠不可靠這類問題?？柭鼮V波相比它們的核心優(yōu)勢有三個(gè)自適應(yīng)權(quán)重?？柭鲆鍷會(huì)隨著協(xié)方差的收斂自動(dòng)變化。剛開始估計(jì)不確定它更相信觀測隨著估計(jì)越來越穩(wěn)它逐漸轉(zhuǎn)向相信模型預(yù)測。遞歸在線處理。你不需要存儲(chǔ)一段歷史數(shù)據(jù)每一幀只需要保留上一幀的狀態(tài)向量和協(xié)方差矩陣計(jì)算量固定非常適合嵌入式實(shí)時(shí)系統(tǒng)。真正利用了系統(tǒng)動(dòng)力學(xué)。模型告訴濾波器系統(tǒng)會(huì)按照某個(gè)規(guī)律演化濾波器也同時(shí)評估這個(gè)規(guī)律本身的可信度。當(dāng)然這些優(yōu)勢是有前提的系統(tǒng)模型近似線性過程噪聲和觀測噪聲近似高斯分布。在這兩個(gè)前提滿足時(shí)卡爾曼濾波在線性高斯意義下就是均方誤差最小的貝葉斯遞歸估計(jì)器。如果系統(tǒng)非線性太強(qiáng)后面會(huì)提到擴(kuò)展卡爾曼和無跡卡爾曼。1.3 卡爾曼濾波的適用邊界與選型判斷不是所有問題都適合卡爾曼濾波。我常跟人講一個(gè)篩選邏輯先問自己三個(gè)問題。第一你是否有一個(gè)可以寫成狀態(tài)遞推的模型比如下一時(shí)刻位置 當(dāng)前速度 × Δt 當(dāng)前位置這就夠了。如果沒有可能更適合滑動(dòng)平均或直接回歸。第二你的傳感器噪聲是不是近似高斯白噪聲偏差bias和粗差尖峰不屬于高斯白噪聲前者需要標(biāo)定或擴(kuò)展?fàn)顟B(tài)估計(jì)后者需要異常值剔除。實(shí)際傳感器大多數(shù)同時(shí)存在兩者。第三你對實(shí)時(shí)性有沒有要求卡爾曼濾波的優(yōu)勢之一是計(jì)算量小非常適合實(shí)時(shí)系統(tǒng)但如果你的數(shù)據(jù)是離線批處理、精度要求極高粒子濾波等非線性方法可能更好。這個(gè)問題定位清楚之后我們再進(jìn)入正題看看怎么把一個(gè)物理問題寫成卡爾曼濾波要求的矩陣形式。2. 寫方程之前先把模型搭起來卡爾曼濾波的核心輸入不是代碼而是兩個(gè)方程狀態(tài)轉(zhuǎn)移方程和觀測方程。我第一次接觸這東西時(shí)急著看公式和代碼結(jié)果發(fā)現(xiàn)代碼跑出來完全不對回頭補(bǔ)了線性系統(tǒng)的知識才明白問題在建模上。所以這一章我必須重點(diǎn)講。2.1 狀態(tài)向量怎么選先說狀態(tài)向量。以一輛直線行駛的汽車為例如果我們只知道位置x模型就只能寫成位置下一幀 位置上一幀 噪聲這個(gè)模型沒有速度信息任何速度變化都被歸進(jìn)了過程噪聲濾波效果會(huì)很差。一個(gè)好的做法是讓狀態(tài)向量包含決定系統(tǒng)演化的所有關(guān)鍵量。對于勻速直線運(yùn)動(dòng)至少是x_k [位置, 速度]^T如果你希望模型允許勻加速可以再加一個(gè)加速度維度x_k [位置, 速度, 加速度]^T這里不是狀態(tài)越多越好。狀態(tài)維度增加計(jì)算量和協(xié)方差矩陣的復(fù)雜度都會(huì)上升而且加速度維度如果本身很不穩(wěn)定模型可能反而不準(zhǔn)。工程里常見的選擇是能用二維位置速度解決的就別堆三維。模型越簡單需要調(diào)的參數(shù)越少發(fā)散的概率越低。2.2 狀態(tài)轉(zhuǎn)移矩陣A和控制輸入B有了狀態(tài)向量下一步寫狀態(tài)轉(zhuǎn)移方程。勻速直線運(yùn)動(dòng)的離散化結(jié)果是p_{k1} p_k v_k Δt v_{k1} v_k寫成矩陣形式x_{k1} [[1, Δt], [0, 1]] x_k w_k這里A矩陣是A [[1, Δt], [0, 1]]它的作用就是把上一幀狀態(tài)按物理規(guī)律外推到當(dāng)前幀。如果模型里還有外部確定的輸入比如剎車帶來的明確加速度可以寫成 B u_k 形式。但對于大多數(shù)狀態(tài)估計(jì)問題外部輸入要么沒有要么已經(jīng)被當(dāng)作隨機(jī)過程處理所以不寫B(tài)項(xiàng)也不會(huì)影響主流程。在Python里用numpy定義A np.array([[1.0, dt], [0.0, 1.0]])就是這個(gè)簡單的2x2矩陣承載了勻速外推這層全部語義。dt是多長時(shí)間由你的采樣周期決定。這個(gè)值直接決定了位置預(yù)測對速度的依賴度很重要?jiǎng)e寫錯(cuò)。2.3 觀測方程和觀測噪聲協(xié)方差R觀測方程描述傳感器測到的到底是什么。GPS測的是位置測不了速度所以觀測矩陣H要把速度分量抽出去H [[1, 0]]公式是z_k H x_k v_kv_k的協(xié)方差R就是你傳感器讀數(shù)的噪聲方差。這個(gè)值怎么確定最實(shí)用的方法是找一個(gè)靜止場景采集幾十到幾百個(gè)觀測值直接算方差。比如把GPS天線放在樓頂固定點(diǎn)采200個(gè)位置點(diǎn)計(jì)算東向和北向坐標(biāo)的方差就能得到R矩陣的對角元素。常有同學(xué)直接把傳感器說明書上的誤差指標(biāo)當(dāng)R用這在論文里可以但在工程里會(huì)翻車——手冊給的是理想環(huán)境指標(biāo)實(shí)際受多徑效應(yīng)、溫度漂移的影響遠(yuǎn)大于手冊值。所以我都會(huì)建議R要么用實(shí)測數(shù)據(jù)算出來要么初始給一個(gè)偏大的值再調(diào)。2.4 過程噪聲Q的物理含義與矩陣構(gòu)造Q表示你對模型本身的信任程度。一個(gè)看似很奇怪但很關(guān)鍵的問題如果系統(tǒng)完全是勻速直線運(yùn)動(dòng)Q是不是應(yīng)該取0理論上是的但現(xiàn)實(shí)中沒有完全勻速的物體——車輪打滑、空氣阻力、路面傾斜都會(huì)帶來未建模的加速度。Q就是為所有你沒寫進(jìn)A矩陣的物理效應(yīng)留的余地。如果Q設(shè)得太小濾波器會(huì)過度相信模型一旦目標(biāo)稍微機(jī)動(dòng)一點(diǎn)估計(jì)就追不上了。如果Q設(shè)得太大濾波器又會(huì)被觀測噪聲帶著走平滑效果全無。在勻加速機(jī)動(dòng)建模中一個(gè)常用的構(gòu)造方式是把加速度當(dāng)作白噪聲推導(dǎo)出的過程噪聲協(xié)方差矩陣是Q σ_a2 × [[Δt?/4, Δt3/2], [Δt3/2, Δt2]]這里的σ_a2是加速度的功率譜密度反映機(jī)動(dòng)強(qiáng)度。你可能好奇為什么位置和速度的協(xié)方差項(xiàng)有一點(diǎn)五次方和平方的關(guān)系——因?yàn)槲恢檬芗铀俣扔绊懯铅2量級速度受加速度影響是Δt量級二者必然在時(shí)間累積上有相關(guān)性。直接記住這個(gè)公式也可以但理解來源對調(diào)參很有幫助。如果你只有一個(gè)標(biāo)量狀態(tài)比如溫度Q就退化成一個(gè)標(biāo)量比如0.01含義是每一步模型預(yù)測的方差是0.01。到這里建模階段完成。你會(huì)發(fā)現(xiàn)并沒有引入什么高深數(shù)學(xué)只是把一個(gè)物理過程老老實(shí)實(shí)地寫成了矩陣遞推。接下來才是那個(gè)讓人頭疼的部分五個(gè)公式到底是怎么來的。3. 卡爾曼濾波五大核心公式的直觀推導(dǎo)卡爾曼濾波的五個(gè)公式看起來像天書但本質(zhì)上就是兩件事的數(shù)學(xué)化先按模型預(yù)測再用觀測修正。這一章我用最直白的方式拆一遍并且給一個(gè)手算數(shù)值例子你看完會(huì)發(fā)現(xiàn)它不過是一套帶權(quán)重的遞推平均。3.1 預(yù)測步驟從上一幀推先驗(yàn)假設(shè)上一幀的最優(yōu)估計(jì)是x?_{k-1}協(xié)方差是P_{k-1}。第一步用狀態(tài)轉(zhuǎn)移矩陣外推先驗(yàn)估計(jì)x?_k^- A x?_{k-1}然后更新先驗(yàn)協(xié)方差P_k^- A P_{k-1} A^T Q這里為什么是A乘P再乘A轉(zhuǎn)置而不是直接乘A因?yàn)閰f(xié)方差的傳播遵循線性變換法則如果 y A x那么 cov(y) A cov(x) A^T?？梢园阉斫鉃檎`差傳播A會(huì)把狀態(tài)的不確定性拉伸和旋轉(zhuǎn)轉(zhuǎn)置的A是為了保持協(xié)方差矩陣的對稱性。這個(gè)兩邊乘A的寫法在工程里非常常見也是很多人在實(shí)現(xiàn)時(shí)最容易漏掉的地方。P矩陣的對角線元素多大了濾波器就有多沒底。初始幀P0設(shè)得大濾波器就知道自己啥都不確定會(huì)放開了信觀測隨著持續(xù)更新P逐漸收斂到一個(gè)較小區(qū)間濾波器也開始更信任自己的預(yù)測。3.2 卡爾曼增益K的核心地位接下來是重頭戲卡爾曼增益K P_k^- H^T (H P_k^- H^T R)^{-1}這個(gè)公式看著嚇人其實(shí)可以逐項(xiàng)拆開理解。H P^- H^T 表示如果把先驗(yàn)估計(jì)投影到觀測空間它的不確定度是多少R是觀測噪聲協(xié)方差。兩者的和是總不確定度。K就是不確定度中來自模型預(yù)測的那一部分占比。如果P^-遠(yuǎn)大于R說明你模型預(yù)測特別沒底而觀測很可靠此時(shí)K接近1濾波結(jié)果幾乎等于觀測值。反過來如果P^-遠(yuǎn)小于R說明你模型預(yù)測已經(jīng)很有把握觀測反而全是噪聲此時(shí)K接近0濾波結(jié)果幾乎等于預(yù)測值。所以K在數(shù)值上一定落在0到1之間標(biāo)量情況它就是一個(gè)動(dòng)態(tài)的信任權(quán)重。第一次見到這個(gè)權(quán)重是自動(dòng)算出來、不用手工設(shè)置時(shí)我是真的覺得這套理論很優(yōu)雅。3.3 更新公式貝葉斯視角下的數(shù)據(jù)融合有了K之后更新分三步x?_k x?_k^- K (z_k - H x?_k^-)P_k (I - K H) P_k^-第一個(gè)公式里 (z_k - H x?_k^-) 叫新息innovation它衡量觀測和預(yù)測之間的差距。如果完全沒有差距說明預(yù)測已經(jīng)完美不需要修正如果差距很大說明預(yù)測嚴(yán)重偏離實(shí)際需要把估計(jì)往觀測方向拉。拉多少乘以K。第二個(gè)公式是協(xié)方差收縮每做一次測量更新P_k只會(huì)比P_k^-小或持平因?yàn)橛^測總是攜帶信息的除非K0。這反映了不確定性的減少也讓濾波器在長期運(yùn)行中保持馴服。值得一提的細(xì)節(jié)是P_k (I - K H) P_k^- 在數(shù)學(xué)上沒問題但在浮點(diǎn)計(jì)算中可能因?yàn)闇p到的實(shí)際數(shù)值太小而打破對稱正定性所以我工程上基本改用Joseph形式P_k (I - K H) P_k^- (I - K H)^T K R K^T這個(gè)式子數(shù)值上穩(wěn)定得多后面代碼里我會(huì)給出用法。3.4 手算一輪迭代把數(shù)字釘進(jìn)腦袋里公式說多了容易暈我們拿一個(gè)標(biāo)量例子完整算一輪。一維常量模型A1H1Q0.01R0.1。初始狀態(tài) x?00P01。第一次觀測值是 z10.5。預(yù)測x?1^- 1 × 0 0P1^- 1 × 1 × 1 0.01 1.01增益K1 1.01 / (1.01 0.1) 0.9099更新x?1 0 0.9099 × (0.5 - 0) 0.4549P1 (1 - 0.9099) × 1.01 0.0909發(fā)現(xiàn)沒有第一輪因?yàn)镻0設(shè)得大濾波器認(rèn)為我的預(yù)測完全不可信而你觀測噪聲只有0.1比你可靠多了所以K接近0.91估計(jì)結(jié)果被觀測牢牢拉住。接著看第二輪。第二次觀測 z20.7。先預(yù)測x?2^- 0.4549P2^- 0.0909 0.01 0.1009增益K2 0.1009 / (0.1009 0.1) 0.5023更新x?2 0.4549 0.5023 × (0.7 - 0.4549) 0.5780P2 (1 - 0.5023) × 0.1009 0.0502這輪的K明顯比第一輪小了。原因很簡單經(jīng)過一輪更新濾波器的預(yù)測不確定性已經(jīng)降到和觀測噪聲差不多同量級所以它不再那么迷信觀測而是把預(yù)測和觀測按約一半一半的比例融合。這個(gè)過程會(huì)持續(xù)P最終會(huì)收斂到一個(gè)穩(wěn)態(tài)值K也趨于穩(wěn)定系統(tǒng)進(jìn)入平衡工作狀態(tài)。理解了這一個(gè)標(biāo)量例子基本上就理解卡爾曼濾波的全部本質(zhì)了。4. Python實(shí)現(xiàn)與仿真驗(yàn)證現(xiàn)在進(jìn)入正題上代碼。我這里給兩個(gè)完整例子一個(gè)一維常量估計(jì)一個(gè)二維位置速度跟蹤。每個(gè)都能直接復(fù)制運(yùn)行你只需要裝好numpy和matplotlib。4.1 通用一維卡爾曼濾波器實(shí)現(xiàn)先寫一個(gè)最簡潔的標(biāo)量版本它雖然只有幾行但已經(jīng)把五個(gè)公式全部包含import numpy as np import matplotlib.pyplot as plt from math import sqrt def kalman_filter_1d(meas, A1.0, H1.0, Q0.01, R1.0, x00.0, P01.0): x x0 P P0 est [] cov [] for z in meas: # 預(yù)測 x_pred A * x P_pred A * P * A Q # 更新 K P_pred * H / (H * P_pred * H R) x x_pred K * (z - H * x_pred) P (1 - K * H) * P_pred est.append(x) cov.append(P) return np.array(est), np.array(cov)這段代碼的A、H、Q、R都是標(biāo)量所以看不出矩陣運(yùn)算的麻煩。它的意義在于讓初學(xué)者一眼看清預(yù)測兩步加更新三步的遞歸結(jié)構(gòu)。函數(shù)返回的是每一幀的估計(jì)值和對應(yīng)的協(xié)方差序列協(xié)方差序列可以拿來觀察收斂過程。4.2 一維場景恒溫箱溫度估計(jì)用這個(gè)函數(shù)來做恒溫箱溫度估計(jì)。真實(shí)溫度設(shè)為23.0°C觀測噪聲方差1.0過程噪聲方差0.01。初始估計(jì)故意設(shè)成0P0設(shè)成1看看濾波器能不能在幾步之內(nèi)從完全錯(cuò)誤的初值追上來。np.random.seed(42) N 100 true_val 23.0 R 1.0 # 模擬100次溫度計(jì)讀數(shù) meas np.random.normal(loctrue_val, scalesqrt(R), sizeN) # 卡爾曼濾波 est, cov kalman_filter_1d(meas, Q0.01, RR, x00.0, P01.0) plt.figure(figsize(10, 4)) plt.plot(meas, alpha0.5, linewidth1, label溫度計(jì)觀測) plt.plot(est, linewidth2, label卡爾曼估計(jì)) plt.axhline(true_val, colorred, linestyle--, linewidth1, label真實(shí)溫度) plt.legend() plt.xlabel(采樣幀) plt.ylabel(溫度 (°C)) plt.title(一維卡爾曼濾波恒溫箱溫度估計(jì)) plt.show() print(穩(wěn)態(tài)協(xié)方差約為:, cov[-1])這段代碼的關(guān)鍵點(diǎn)在于觀測值的野跳非常明顯但估計(jì)曲線幾乎只在23°C附近輕微浮動(dòng)。前幾幀從0快速逼近23的過程對應(yīng)的是協(xié)方差收縮和增益K從0.9向0.09快速下降的過程。跑完你會(huì)發(fā)現(xiàn)一個(gè)有趣的對比單純看單個(gè)觀測樣本最大值可能沖到25°C最小值可能掉到21°C但濾波輸出基本在22.9到23.1之間。這說明卡爾曼濾波確實(shí)把觀測噪聲抑制掉了一大部分代價(jià)是響應(yīng)略微平滑。對于恒溫箱這種緩慢變化的對象這種平滑正是我們想要的。4.3 二維場景位置-速度跟蹤完整實(shí)現(xiàn)再升一個(gè)維度寫位置-速度聯(lián)合估計(jì)。目標(biāo)是讓濾波器在只觀測位置的情況下同時(shí)估計(jì)出速度。這個(gè)場景非常典型本質(zhì)上就是GPS測位但不測速而我們需要知道速度來完成導(dǎo)航。dt 0.1 N 300 # 真實(shí)軌跡前150幀勻加速后150幀減速模擬機(jī)動(dòng) true_pos np.zeros(N) true_vel np.zeros(N) for i in range(N - 1): if i 150: a 0.5 else: a -0.3 true_vel[i 1] true_vel[i] a * dt true_pos[i 1] true_pos[i] true_vel[i 1] * dt # 觀測只有位置標(biāo)準(zhǔn)差 sqrt(0.25)0.5m r 0.25 meas true_pos np.random.normal(0, sqrt(r), N) # 卡爾曼濾波 q 0.2 A np.array([[1.0, dt], [0.0, 1.0]]) H np.array([[1.0, 0.0]]) Q np.array([[q * dt**4 / 4, q * dt**3 / 2], [q * dt**3 / 2, q * dt**2]]) R np.array([[r]]) x np.array([0.0, 0.0]) # 初始位置、速度 P np.eye(2) * 10.0 # 初始協(xié)方差給大一點(diǎn) est_pos np.zeros(N) est_vel np.zeros(N) for i, z in enumerate(meas): # 預(yù)測 x_pred A x P_pred A P A.T Q # 更新 S H P_pred H.T R K P_pred H.T np.linalg.inv(S) innovation z - (H x_pred).item() x x_pred K.ravel() * innovation P (np.eye(2) - K H) P_pred est_pos[i] x[0] est_vel[i] x[1] # 評估 rmse sqrt(np.mean((est_pos - true_pos)**2)) print(f位置估計(jì)RMSE: {rmse:.4f} m) plt.figure(figsize(10, 4)) plt.plot(true_pos, linewidth2, label真實(shí)位置) plt.plot(meas, alpha0.4, linewidth1, label帶噪觀測) plt.plot(est_pos, linewidth1.5, linestyle--, label卡爾曼估計(jì)位置) plt.legend() plt.xlabel(采樣幀) plt.ylabel(位置 (m)) plt.title(二維卡爾曼濾波位置-速度聯(lián)合估計(jì)) plt.show()這里有幾個(gè)實(shí)現(xiàn)細(xì)節(jié)需要注意。首先是K的維度。K是2x1的矩陣innovation是標(biāo)量所以用K.ravel()把它拉平再乘標(biāo)量得到2維修正向量。如果你寫成K z這種形式記得把z包成1x1矩陣或者像我這樣直接處理為標(biāo)量更直觀。其次Q矩陣的構(gòu)造用了白噪聲加速度模型。q0.2意味著加速度不確定性在一個(gè)周期內(nèi)對位置引入約0.2×Δt?/4的方差這個(gè)數(shù)值比觀測噪聲的0.25略小表示我們愿意讓濾波器相信目標(biāo)在大部分時(shí)間里運(yùn)動(dòng)是規(guī)律的。如果q太小那后半段減速機(jī)動(dòng)的時(shí)候估計(jì)會(huì)滯后很多。再看RMSE的結(jié)果。在這個(gè)參數(shù)下觀測噪聲標(biāo)準(zhǔn)差是0.5m濾波后的位置RMSE通常在0.2到0.3m之間改善明顯。速度估計(jì)的均方誤差也相當(dāng)?shù)碗m然沒有任何傳感器直接測速度但濾波器通過位置的差分加模型約束把速度推出來了。4.4 輸出結(jié)果怎么看收斂、平滑與跟蹤滯后跑完代碼重點(diǎn)看三件事。第一是估計(jì)曲線和真實(shí)位置的重合度。如果估計(jì)曲線在機(jī)動(dòng)段第150幀附近明顯落后真值說明q太小濾波器過于信任勻速模型。如果估計(jì)曲線瘋狂抖動(dòng)和觀測曲線幾乎重合說明q太大濾波基本失去了平滑能力。第二是初始幾幀的快速修正。二維代碼里P0設(shè)成10倍單位矩陣目的就是讓濾波器在最初的幾幀內(nèi)快速把誤差消化掉。你可以試試把P0改成0.01再把初始位置從0開始但真值在50m處你會(huì)發(fā)現(xiàn)濾波器會(huì)長時(shí)間貼著錯(cuò)的初值跑不動(dòng)。這就是初始協(xié)方差太小導(dǎo)致早期無法修正的經(jīng)典現(xiàn)象。第三是速度估計(jì)的滯后。位置跟蹤的滯后看起來不明顯但速度估計(jì)對機(jī)動(dòng)反饋慢很多。如果你把q調(diào)大速度響應(yīng)會(huì)變快但噪聲同時(shí)變大調(diào)小則反過來。這就是卡爾曼濾波里最經(jīng)典的響應(yīng)速度 vs 平滑程度的權(quán)衡本質(zhì)上是Q和R的博弈。5. 參數(shù)調(diào)優(yōu)、發(fā)散問題與工程中的坑代碼能跑只是起點(diǎn)。真正讓卡爾曼濾波好用的是參數(shù)調(diào)試和異常處理。這一章全是我的實(shí)際經(jīng)驗(yàn)每一個(gè)坑都踩過。5.1 P0、Q、R三個(gè)矩陣各自的調(diào)參手感先說P0。它的影響主要在最初幾十幀。工程上我的習(xí)慣是對完全沒把握的初始狀態(tài)P0給到對角線10到100級別讓濾波器先犯錯(cuò)再快速修正。如果你有前幾個(gè)靜態(tài)幀數(shù)據(jù)也可以用第一個(gè)觀測值的方差來初始化這樣更穩(wěn)。R的調(diào)法最簡單不要拍腦袋去測。把傳感器固定不動(dòng)讀取200個(gè)靜態(tài)樣本算方差那就是R的下界。如果實(shí)際場景中傳感器噪聲會(huì)因環(huán)境變化而增大我習(xí)慣把R設(shè)置成靜態(tài)方差的兩倍留點(diǎn)安全余量。R設(shè)小了濾波器會(huì)過度信任觀測在傳感器漂移時(shí)會(huì)掛得很慘。Q的調(diào)法最難因?yàn)樗举|(zhì)上是你所有未建模誤差的匯總。我調(diào)Q時(shí)一般遵循這個(gè)流程先給一個(gè)偏小的Q比如0.01或0.1看觀測殘差是否顯著大于理論值。如果殘差大說明模型欠配Q偏小如果殘差正常但估計(jì)曲線抖動(dòng)說明Q偏大噪聲被放進(jìn)來了。反復(fù)迭代幾次直到殘差統(tǒng)計(jì)和理論吻合。這里給一張我常用的速查表參數(shù)偏離情況典型表現(xiàn)調(diào)整方向Q相對R過小估計(jì)曲線過于平滑機(jī)動(dòng)跟不住殘差系統(tǒng)性偏大增大QQ相對R過大估計(jì)曲線抖動(dòng)嚴(yán)重幾乎跟著觀測走減小QR設(shè)得過小濾波器過于信任觀測傳感器一抖就跟著抖增大RR設(shè)得過大濾波器過于信任模型突發(fā)測量變化被忽略減小RP0過小且初值錯(cuò)誤初始跟蹤極慢長時(shí)間飛不到真值附近增大P0別小看這張表我見過太多人在真實(shí)項(xiàng)目里被Q/R比值要多少折磨。沒有萬能參數(shù)只有通過殘差分析才能確定。5.2 濾波發(fā)散的具體表現(xiàn)與背后原因發(fā)散是卡爾曼濾波最讓人崩潰的問題估計(jì)結(jié)果飛上天完全脫離真值。本質(zhì)原因只有一個(gè)——模型和觀測的統(tǒng)計(jì)特性假設(shè)與實(shí)際不符而這個(gè)不匹配導(dǎo)致協(xié)方差P持續(xù)更新錯(cuò)誤。最常見的發(fā)散場景有三個(gè)。第一個(gè)是Q過小目標(biāo)機(jī)動(dòng)。目標(biāo)在轉(zhuǎn)彎或變道但模型假設(shè)勻速。每一幀的新息都是正的觀測在預(yù)測方向之前濾波器雖然會(huì)修正但每次修正量都被小Q壓住速度估計(jì)始終追不上位置誤差越來越大。表現(xiàn)就是開車時(shí)導(dǎo)航位置一直在后面跟著。第二個(gè)是R被嚴(yán)重低估。你給了傳感器一個(gè)比實(shí)際小很多的R濾波器就會(huì)非常自信地接受觀測噪聲等效于把噪聲整個(gè)放進(jìn)了估計(jì)。表現(xiàn)是估計(jì)軌跡毛刺極多且協(xié)方差矩陣收斂到一個(gè)很小但完全不真實(shí)的數(shù)值。發(fā)散時(shí)P矩陣甚至可能違背直覺地不斷變小這是最隱蔽的。第三個(gè)是數(shù)值問題引發(fā)的協(xié)方差非正定。比如使用了P (I - KH)P^-這個(gè)簡化形式并長時(shí)間循環(huán)浮點(diǎn)誤差可能讓協(xié)方差的對角線出現(xiàn)負(fù)值或非對稱。解決方法是改用Joseph形式并且定期強(qiáng)制對稱P (np.eye(2) - K H) P_pred (np.eye(2) - K H).T K R K.T P (P P.T) / 2再給一個(gè)診斷小技巧保存每一幀的新息innovation序列計(jì)算它的均值和方差和理論值 HPH^TR 做對比。如果均值明顯偏離零或者實(shí)際方差遠(yuǎn)大于理論方差基本能鎖定模型的失配問題。5.3 異常值處理卡方門限與自適應(yīng)R傳感器偶爾會(huì)冒出離譜的粗差——GPS在隧道里信號跳變、毫米波雷達(dá)被強(qiáng)反射干擾都是真實(shí)場景的常態(tài)。如果讓這種觀測正常參與濾波結(jié)果就是位置瞬間被拉飛要好幾個(gè)周期才能收回來。我的做法是在更新前加一個(gè)粗差檢測門。核心思想是如果新息和它的理論協(xié)方差不相容就認(rèn)為這個(gè)觀測是異常的直接丟棄或用另一個(gè)放大的R更新。具體方法是用馬氏距離。新息 e z - H x?^-其理論協(xié)方差為 S H P^- H^T R。計(jì)算門限量g e^T S^{-1} e如果系統(tǒng)是真高斯分布這個(gè)g應(yīng)該服從自由度等于觀測維度的卡方分布。比如觀測維度是1時(shí)95%置信的門限是3.8499%是6.63。如果g超過門限就說明觀測與模型預(yù)測不一致的概率很大。簡單的工程實(shí)現(xiàn)S H P_pred H.T R e z - (H x_pred).item() g e * e / S.item() if g 6.63: # 卡方0.99門限 # 正常更新 K P_pred H.T np.linalg.inv(S) x x_pred K.ravel() * e else: # 丟棄該觀測純預(yù)測 x x_pred這個(gè)方法簡單又好用。不過要注意極端情況如果目標(biāo)真的在劇烈機(jī)動(dòng)并且你的Q設(shè)得很小那么每一幀的新息都會(huì)很大卡方門限會(huì)把所有觀測全部拒掉濾波器就完全癱了。所以門限檢測要和合理的Q配套使用同時(shí)做加速度突變的檢測二者結(jié)合才安全。5.4 數(shù)值穩(wěn)定性求逆用Cholesky還是inv寫代碼時(shí)很多同學(xué)喜歡直接用np.linalg.inv(S)低維度下很少出問題但工程上我不建議這么干原因有兩個(gè)。一是效率。一維二維問題無所謂但狀態(tài)維度上了10S的維度也上去了inv的計(jì)算量是O(n3)而卡爾曼濾波是實(shí)時(shí)系統(tǒng)能省則省。二是數(shù)值穩(wěn)定性。S通常是對稱正定矩陣用inv求逆可能由于舍入誤差引入不對稱性。我習(xí)慣用Cholesky分解或者numpy.linalg.solve# 用solve代替invK P_pred H.T inv(S) # 等價(jià)于解線性方程組 S K.T H P_pred.T K_T np.linalg.solve(S, H P_pred.T) K K_T.T這個(gè)寫法的數(shù)值穩(wěn)定性比直接inv好一些而且代碼更貼近矩陣方程的數(shù)學(xué)本質(zhì)。如果你的狀態(tài)量是標(biāo)量那就無所謂了直接除法就行。除了求逆另一個(gè)數(shù)值坑是協(xié)方差矩陣在長期運(yùn)行后變得不對稱甚至非正定。我上面的代碼里已經(jīng)加了強(qiáng)制對稱處理這在小規(guī)模仿真中看不出來但在長時(shí)間運(yùn)行的嵌入式系統(tǒng)里作用巨大。5.5 擴(kuò)展卡爾曼濾波EKF和無跡卡爾曼濾波UKF的方向如果你的系統(tǒng)模型不是線性的經(jīng)典的卡爾曼濾波就不適用了。典型的非線性場景有兩類一類是傳感器模型非線性比如雷達(dá)給出的是距離和方位角而狀態(tài)是直角坐標(biāo)另一類是狀態(tài)轉(zhuǎn)移本身非線性比如無人機(jī)姿態(tài)動(dòng)力學(xué)。處理非線性的主流路數(shù)有兩種擴(kuò)展卡爾曼濾波EKF是對非線性方程在估計(jì)點(diǎn)附近做一階泰勒展開本質(zhì)上是每個(gè)時(shí)刻都重新線性化。優(yōu)點(diǎn)是實(shí)現(xiàn)簡單、計(jì)算量小缺點(diǎn)是一階近似在強(qiáng)非線性下精度差而且需要推導(dǎo)雅可比矩陣有點(diǎn)繁瑣。無跡卡爾曼濾波UKF不線性化函數(shù)而是生成一批稱為sigma點(diǎn)的采樣點(diǎn)讓它們通過非線性函數(shù)再從變換后的點(diǎn)中重構(gòu)均值和協(xié)方差。它在精度和實(shí)現(xiàn)難度之間是很均衡的選擇不需要推導(dǎo)雅可比矩陣非線性較強(qiáng)時(shí)也比EKF穩(wěn)定。但如果你的問題真的極度非線性、多峰值比如從地圖匹配中做全局定位那應(yīng)該考慮粒子濾波?？柭易褰鉀Q的是高斯單峰假設(shè)下的最優(yōu)問題在多模態(tài)場景下會(huì)整體失效。這一節(jié)不是簡單的知識延伸。我的建議是一定要先理解線性卡爾曼再上非線性否則你連EKF哪里近似都不知道出問題更不會(huì)排查。很多同學(xué)直接上手EKF結(jié)果完全跑偏回頭發(fā)現(xiàn)是線性化點(diǎn)選錯(cuò)了后悔不迭。最后分享一個(gè)我在實(shí)際項(xiàng)目里反復(fù)驗(yàn)證過的習(xí)慣拿到新系統(tǒng)先用一個(gè)很小的數(shù)據(jù)集把濾波器跑通把真值、觀測、估計(jì)三條曲線和殘差圖畫出來確認(rèn)邏輯正確再談?wù){(diào)參。不放圖就跑仿真很難發(fā)現(xiàn)Q/R方向拿沒拿對。另一個(gè)習(xí)慣是把濾波器封裝成獨(dú)立類方便在不同傳感器之間切換和對比不同變體?？柭鼮V波表面上是五個(gè)公式真正的難點(diǎn)永遠(yuǎn)是建模和噪聲刻畫這兩樣做好了代碼只是幾行公式翻譯而已。