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