簡介卡爾曼濾波是處理含噪動態系統狀態估計與多傳感器數據融合的經典遞歸算法面向信號處理、控制工程以及機器學習方向的學習者與開發者提供一套基于MATLAB的卡爾曼濾波數據融合演示代碼。資源包共2個文件包含1個m腳本和1個txt配套說明文件整體大小僅2KB腳本用于實現多源觀測信息的融合與遞推更新文本文件則可作為輸入數據或參數配置參考適合在理解算法原理后動手運行與調參驗證。資源描述對基本原理亦有清晰梳理例如系統狀態服從高斯分布、通過預測和更新兩個步驟相接合獲得最優估計并將應用場景延伸至模式識別序列數據跟蹤、函數逼近與在線參數估計方便讀者從單一示例擴展到實際項目。雖然壓縮包體積很小但演示鏈路完整能夠覆蓋從模型構建、觀測輸入到狀態輸出的全流程。已有655人學習瀏覽通過運行該腳本可以直觀觀察預測與更新迭代對狀態估計的影響理解噪聲協方差與權重分配的作用為后續實現更復雜的多傳感器融合系統打下扎實基礎。1. 兩個傳感器數據打架時卡爾曼濾波做的不是平均而是建模做 datsfusion 這類多源融合任務時我第一次拿到兩路同時測同一個位置的傳感器數據直覺反應是加權平均噪聲大的少乘一點噪聲小的多乘一點。結果一跑就發現靜態場景勉強能用目標一加速輸出立刻滯后半拍傳感器噪聲稍微變一下權重又要重新調。卡爾曼濾波把這個問題的解法換了一個方向——它不直接決定“信誰多少”而是給兩個傳感器各建一個噪聲模型再按模型的不確定度動態生成融合權重。哪個傳感器此刻更可信是由協方差遞推算出來的不是預先寫死的。這篇文章就沿著這個思路把狀態方程、Q/R 協方差設定、最小可復現代碼、擴展到目標跟蹤的 EKF 路徑以及現場調參的驗證手段一路講到底。適合做定位、導航、多傳感器融合以及卡爾曼濾波目標跟蹤的工程師新手能跟著跑通代碼熟手可以重點看后兩章的參數邊界和坑點。2. 數據融合用卡爾曼濾波先把狀態方程寫對噪聲協方差才有意義2.1 狀態空間表達式把多傳感器觀測統一到同一個狀態向量里卡爾曼濾波處理數據融合第一步不是寫濾波公式而是把物理問題翻譯成狀態空間表達式。狀態方程x_k A x_{k-1} B u_k w_k 測量方程z_k H x_k v_kx 是狀態向量它描述系統在某一時刻的完整內部狀態。做數據融合時不同傳感器觀測的是同一個狀態的不同投影GPS 直接給位置里程計給速度積分加速度計給比力。如果把它們各自獨立地拿來做估計得到的是好幾個互相不一致的答案放進卡爾曼濾波框架后所有傳感器都對應到同一個 x 上只是各自的 H 不同。H 的物理含義是“狀態到測量的映射”GPS 的 H 是位置那幾列取 1里程計的 H 是速度那幾列取 1。w 和 v 分別代表過程噪聲與測量噪聲它們各自的協方差矩陣 Q 和 R才是后續卡爾曼增益計算里決定融合權重的核心。常見做法是先在紙上把狀態向量列出來再逐個傳感器寫 H最后才碰代碼。我見過不少融合結果發散的項目根源都不是濾波公式寫錯而是狀態向量順序和 H 矩陣列沒對齊導致預測和更新在融合兩個毫不相干的量。2.2 Q 和 R 的物理語義卡爾曼增益背后的融合權重分配卡爾曼增益 K 的表達式是K P H^T (H P H^T R)^-1K 的數值決定了最終輸出在“預測值”和“測量值”之間取哪里。P 是狀態協方差矩陣它反映當前對狀態估計的不確定度R 是測量噪聲協方差反映傳感器讀數本身的可信度。當 R 相對于 P 很大時K 趨近于 0濾波器更相信模型預測當 R 相對于 P 很小時K 趨近于 1濾波器更相信傳感器。這就是數據融合用卡爾曼濾波的本質它不做固定權重的平均而是讓權重隨不確定度實時變化。某一時刻 GPS 信號抖動劇烈R 變大融合結果自然偏向里程計下一秒 GPS 恢復權重又自動切回來。Q 描述的是模型本身的不可信程度。如果系統實際在加速而狀態方程用的是勻速模型那么 Q 就要留出足夠的余量來容納這種未建模的動態Q 設得太小濾波器會過度信任預測表現為輸出平滑但滯后嚴重。Q 設得太大融合結果又會被過程噪聲淹沒噪聲抑制效果退化成一階低通濾波甚至更差。提示卡爾曼濾波和常見的一階低通濾波的差別就在這個 K 是自適應計算的。一階低通濾波的系數是常數卡爾曼的“系數”是隨 P 和 R 不斷更新的變量。2.3 定義融合系統的狀態向量與測量矩陣一張表說清維度對應以 datsfusion 里最常見的二維定位融合為例狀態向量一般取四個量x 方向位置、x 方向速度、y 方向位置、y 方向速度。GPS 輸出位置里程計輸出速度H 矩陣就要按這個順序去對齊。狀態向量成員物理含義對應傳感器測量方程中的體現p_xx 軸位置GPSz_gps p_x v_gpsv_xx 軸速度里程計/IMUz_odom v_x v_odomp_yy 軸位置GPSz_gps p_y v_gpsv_yy 軸速度里程計/IMUz_odom v_y v_odom狀態向量一旦含速度A 矩陣自然就是勻速模型的形式位置列里帶一個 dt速度列保持 1。很多初學者把狀態向量只定義為位置用兩個位置傳感器做融合也能跑通但融合結果沒有速度估計后續做目標跟蹤時就沒有預測能力每次都要等測量到了才能動。既然做的是數據融合能多狀態量就多狀態量這樣 Q 矩陣能把速度的變化過程也建模進去預測出來的軌跡才連貫。H 矩陣的維度是“測量數 × 狀態維數”。GPS 和里程計同時到達時測量向量是兩維H 是 2×4 的矩陣每一行對應一個傳感器的觀測方程。寫完第一步就是檢查矩陣維度能不能相乘這一步錯了后面代碼跑起來全是維度報錯但報錯信息往往會把你引向完全無關的地方。3. 用卡爾曼濾波做數據融合一個能直接跑的最小 Python 實現3.1 最小卡爾曼濾波類predict 與 update 兩個核心方法先寫一個一維的單變量卡爾曼濾波器類再看它怎么融合兩路傳感器。單變量場景里所有矩陣都退化成標量邏輯最直觀。import numpy as np class KalmanFilter1D: 一維卡爾曼濾波用于說明 predict / update 的完整流程 def __init__(self, q0.01, p01.0): self.q q # 過程噪聲方差模型未建模動態的余量 self.p p0 # 初始協方差給大一點可以加速收斂 self.x 0.0 # 初始狀態估計 def predict(self, dt1.0): # 先驗估計狀態不變協方差累加過程噪聲 self.p self.p self.q * dt def update(self, z, r): # z 是傳感器讀數r 是該傳感器的測量噪聲方差 k self.p / (self.p r) # 卡爾曼增益 self.x self.x k * (z - self.x) # 用新息修正狀態 self.p (1 - k) * self.p # 更新后協方差 return self.x這里卡爾曼增益 k 的表達式是一維化的形式K P / (P R)。當 R 遠大于 Pk 趨近于 0測量修正量很小當 R 遠小于 Pk 趨近于 1狀態幾乎完全跟隨測量。update 里的(z - self.x)是新息表示“測量值與當前預測的差值”卡爾曼濾波的全部修正動作都建立在這個差值上。r 參數按傳感器分別傳入這給了數據融合一個天然的結構同一個濾波器不同傳感器各自帶自己的噪聲方差。3.2 雙傳感器融合的最小場景GPS 與里程計同時輸出位置現在構造一個仿真場景目標真實位置是恒定值 5.0GPS 噪聲標準差 2.0里程計噪聲標準差 0.5。注意這里用標準差構造數據傳入濾波器時要把標準差平方成方差。np.random.seed(42) true_pos 5.0 n 100 # 兩路傳感器觀測同一位置噪聲水平差異很大 gps true_pos np.random.normal(0, 2.0, n) # GPS大噪聲 odom true_pos np.random.normal(0, 0.5, n) # 里程計小噪聲 kf KalmanFilter1D(q0.01, p01.0) states [] for t in range(n): kf.predict(dt1.0) kf.update(gps[t], r2.0**2) # GPS 的 R 為 4.0 kf.update(odom[t], r0.5**2) # 里程計 R 為 0.25 states.append(kf.x) # 對比純 GPS 均值誤差 vs 融合結果誤差 gps_err np.abs(np.mean(gps) - true_pos) fusion_err np.abs(states[-1] - true_pos) print(GPS 均值誤差: %.3f, 卡爾曼融合誤差: %.3f % (gps_err, fusion_err))同一時刻兩路測量到達可以連續調用兩次 update。第一次用 GPS 更新協方差 P 會縮小第二次用里程計更新是在 GPS 更新完的基礎上繼續修正。順序本身不影響最終結果因為卡爾曼遞推滿足結合律但每次 update 前不要忘記先 predict 一次。r 參數要按傳感器分別傳入GPS 的 R 是 4.0里程計是 0.25后者權重天然更大。最終融合誤差通常遠小于純 GPS 的均值誤差這就是把測量噪聲模型化之后得到的收益。注意R 傳方差還是標準差是新手最容易踩的坑。傳感器手冊給的一般是“精度 ±0.5m”這是標準差傳進公式時要平方成 0.25。不平方也能跑但濾波結果對噪聲的抑制程度會整體失真。3.3 單變量到多維融合的改動邊界同一套公式換矩陣維度一維場景跑通后二維多維融合的代碼結構不用變只把標量換成矩陣。狀態向量 x 變成 4×1A 變成 4×4P 變成 4×4H 按傳感器測量維度取對應的行。predict 里做F P F.T Qupdate 里先算S H P H.T R再算K P H.T np.linalg.inv(S)。建議在代碼里把矩陣維度寫清楚后先用單位矩陣做一次冒煙測試狀態向量全取 1觀測全取 1跑幾步看輸出是否發散。發散就逐項檢查 A、H 的維度配對而不是直接懷疑濾波公式。矩陣融合和多傳感器融合里 90% 的運行時錯誤都是維度錯位一維代碼很難暴露這類問題。4. 擴展卡爾曼濾波與目標跟蹤非線性融合場景的升級路徑4.1 為什么目標跟蹤里要用擴展卡爾曼濾波而不是線性卡爾曼線性卡爾曼的前提是狀態轉移和測量方程都是線性的。但目標跟蹤場景里傳感器輸出的常常不是位置坐標而是距離和角度比如雷達測出(r, θ)換算成直角坐標是px r * cos(θ), py r * sin(θ)。這個映射關系天然帶三角函數不是線性方程。強行用線性卡爾曼的結果是測量更新這一步的 H 矩陣沒法表達真實的映射融合輸出會在目標轉彎時出現明顯偏差。擴展卡爾曼濾波EKF的處理方式是對非線性函數做一階泰勒展開用雅可比矩陣代替原來的線性 H 矩陣。每一時刻的 H 都不同它依賴當前狀態估計值所以要在每次更新時重新計算。這個“重新算 H”的動作就是 EKF 比線性卡爾曼多出來的核心計算量。4.2 雷達測距測角場景的 EKF 預測與更新代碼狀態仍然取[px, py, vx, vy]過程模型繼續用勻速模型測量模型改成非線性函數并顯式寫出雅可比矩陣。import numpy as np def ekf_predict(x, P, dt, Q): # 勻速模型的狀態轉移矩陣 F np.array([ [1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1] ]) x F x P F P F.T Q return x, P def ekf_update(x, P, z, R): px, py, vx, vy x r np.sqrt(px**2 py**2) # 雅可比矩陣測量方程對狀態求偏導 H np.array([ [px / r, py / r, 0, 0], [-py / (r**2), px / (r**2), 0, 0] ]) # 預測測量值 z_pred np.array([r, np.arctan2(py, px)]) # 新息協方差、增益、狀態與協方差更新 S H P H.T R K P H.T np.linalg.inv(S) innovation z - z_pred # 角度新息要歸一化到 [-pi, pi]否則角度跳變會擊穿濾波器 innovation[1] (innovation[1] np.pi) % (2 * np.pi) - np.pi x x K innovation P (np.eye(4) - K H) P return x, P, innovation雅可比矩陣的第一行是距離 r 對位置分量的偏導第二行是角度 θ 對位置分量的偏導。速度分量在測量方程里不出現對應位置全為 0。角度新息的歸一化是目標跟蹤里必須寫的細節角度從 179 度跳到 -179 度數值上變化了 358 度但真實角度變化只有 2 度。不做歸一化這個虛假的大新息會把濾波器狀態直接沖偏。4.3 融合發散前看這三個指標新息、協方差對角線、誤差包絡EKF 跑起來之后不要只盯著最終輸出曲線看融合是否發散應該用三個量化指標預判。新息的均值應該接近 0方差應該接近 S 矩陣的對角線。如果新息長期偏向一側說明模型有系統偏差比如勻速模型跟不上轉彎目標如果新息方差遠大于 S 的預期說明 R 設得太小濾波器過度相信測量。P 矩陣對角線代表每個狀態量的估計方差理想情況下應該逐步收斂并保持穩定。P 持續增長不下降說明更新環節沒有起到修正作用通常是 H 算錯或者 R 設置過大P 快速收斂到極小值往往是過度自信后續一旦出現異常測量濾波器根本來不及反應。誤差包絡是實際測量殘差與理論方差的比值。工程上常用一個移動窗口統計實際新息的標準差與 S 的理論值做對比比值在 1.5 倍以內算正常超過 3 倍基本可以判定濾波器模型失真需要回頭檢查 Q 和 R 的匹配度。5. 數據融合現場先調這三個參數再用新息驗證濾波效果5.1 R 矩陣標定用靜態數據算方差別靠手感R 是唯一能通過實驗直接測出來的參數。做法是把傳感器放在靜止環境下采集 100 到 200 個數據點直接算方差。代碼就三行samples np.array([...]) # 靜止時采集的傳感器讀數 r np.var(samples) # 測量噪聲方差靜態標定得到的 R 是基線值實際運行中還要根據場景微調。比如 GPS 在城市峽谷里噪聲會顯著增大這時候 R 按靜態值給濾波器會把嚴重偏離的測量當成正常數據吸收進去。工程上的常見做法是做一個簡單的環境判斷當新息連續多步超過 3 倍標準差時臨時把 R 放大 5 到 10 倍等新息回歸正常再切回來。5.2 Q 矩陣調參從滯后到跟手的現場步驟Q 沒有直接測量手段通常靠效果反推。調參步驟可以按下面這張表執行現象原因判斷調整方向輸出平滑但滯后嚴重Q 偏小模型過度自信Q 增大 3~5 倍讓預測跟隨動態輸出抖動明顯噪聲沒壓住Q 偏大或 R 偏小先驗證 R再減小 Q新息長期不為零模型誤差或 Q 太小Q 增大或檢查狀態轉移方程P 收斂后不下降R 過大按標定值重新給定 R調整 Q 時建議按對角元素逐項調不要整體縮放。位置對應的 Q 和速度對應的 Q 物理意義不同整體縮放一次往往會把原來合理的部分也弄壞。拿目標跟蹤來說位置過程噪聲來自目標加速度的隨機變化速度過程噪聲還包含模型本身的截斷誤差兩項要給不同的數量級。5.3 新息序列驗證與在線噪聲協方差估計的小技巧調完 Q 和 R 后用一個移動窗口持續統計新息的均值和方差。均值應該在 0 附近波動窗口方差與 S 的理論值匹配。這里有一個實用的在線自適應技巧用滑動窗口估計實際新息方差sigma_hat如果它持續大于 S說明 R 被低估了用兩者的比值去修正 Rwindow_size 50 innovation_history [] # 每步更新后追加新息 innovation_history.append(innovation[0]) if len(innovation_history) window_size: innovation_history.pop(0) # 窗口內的實際新息標準差與理論 S 對比 sigma_hat np.std(innovation_history) if sigma_hat 2.0 * np.sqrt(S[0, 0]): R_adapted R_original * (sigma_hat / np.sqrt(S[0, 0]))**2這個比值修正法在噪聲緩變場景里非常有效能自動適應傳感器性能退化。但它對環境突變不敏感如果目標突然大幅加速新息會驟增這時更合適的做法是臨時增大 Q 而不是調整 R。判斷依據很簡單目標動態變了新息增大但協方差 P 正常傳感器壞了新息和 P 同步惡化。把這兩個信號區分開融合系統才算真正具備現場可維護性。本文還有配套的精品資源點擊獲取 卡爾曼濾波多傳感器融合實戰：從狀態方程到EKF目標跟蹤調參
簡介卡爾曼濾波是處理含噪動態系統狀態估計與多傳感器數據融合的經典遞歸算法面向信號處理、控制工程以及機器學習方向的學習者與開發者提供一套基于MATLAB的卡爾曼濾波數據融合演示代碼。資源包共2個文件包含1個m腳本和1個txt配套說明文件整體大小僅2KB腳本用于實現多源觀測信息的融合與遞推更新文本文件則可作為輸入數據或參數配置參考適合在理解算法原理后動手運行與調參驗證。資源描述對基本原理亦有清晰梳理例如系統狀態服從高斯分布、通過預測和更新兩個步驟相接合獲得最優估計并將應用場景延伸至模式識別序列數據跟蹤、函數逼近與在線參數估計方便讀者從單一示例擴展到實際項目。雖然壓縮包體積很小但演示鏈路完整能夠覆蓋從模型構建、觀測輸入到狀態輸出的全流程。已有655人學習瀏覽通過運行該腳本可以直觀觀察預測與更新迭代對狀態估計的影響理解噪聲協方差與權重分配的作用為后續實現更復雜的多傳感器融合系統打下扎實基礎。1. 兩個傳感器數據打架時卡爾曼濾波做的不是平均而是建模做 datsfusion 這類多源融合任務時我第一次拿到兩路同時測同一個位置的傳感器數據直覺反應是加權平均噪聲大的少乘一點噪聲小的多乘一點。結果一跑就發現靜態場景勉強能用目標一加速輸出立刻滯后半拍傳感器噪聲稍微變一下權重又要重新調。卡爾曼濾波把這個問題的解法換了一個方向——它不直接決定“信誰多少”而是給兩個傳感器各建一個噪聲模型再按模型的不確定度動態生成融合權重。哪個傳感器此刻更可信是由協方差遞推算出來的不是預先寫死的。這篇文章就沿著這個思路把狀態方程、Q/R 協方差設定、最小可復現代碼、擴展到目標跟蹤的 EKF 路徑以及現場調參的驗證手段一路講到底。適合做定位、導航、多傳感器融合以及卡爾曼濾波目標跟蹤的工程師新手能跟著跑通代碼熟手可以重點看后兩章的參數邊界和坑點。2. 數據融合用卡爾曼濾波先把狀態方程寫對噪聲協方差才有意義2.1 狀態空間表達式把多傳感器觀測統一到同一個狀態向量里卡爾曼濾波處理數據融合第一步不是寫濾波公式而是把物理問題翻譯成狀態空間表達式。狀態方程x_k A x_{k-1} B u_k w_k 測量方程z_k H x_k v_kx 是狀態向量它描述系統在某一時刻的完整內部狀態。做數據融合時不同傳感器觀測的是同一個狀態的不同投影GPS 直接給位置里程計給速度積分加速度計給比力。如果把它們各自獨立地拿來做估計得到的是好幾個互相不一致的答案放進卡爾曼濾波框架后所有傳感器都對應到同一個 x 上只是各自的 H 不同。H 的物理含義是“狀態到測量的映射”GPS 的 H 是位置那幾列取 1里程計的 H 是速度那幾列取 1。w 和 v 分別代表過程噪聲與測量噪聲它們各自的協方差矩陣 Q 和 R才是后續卡爾曼增益計算里決定融合權重的核心。常見做法是先在紙上把狀態向量列出來再逐個傳感器寫 H最后才碰代碼。我見過不少融合結果發散的項目根源都不是濾波公式寫錯而是狀態向量順序和 H 矩陣列沒對齊導致預測和更新在融合兩個毫不相干的量。2.2 Q 和 R 的物理語義卡爾曼增益背后的融合權重分配卡爾曼增益 K 的表達式是K P H^T (H P H^T R)^-1K 的數值決定了最終輸出在“預測值”和“測量值”之間取哪里。P 是狀態協方差矩陣它反映當前對狀態估計的不確定度R 是測量噪聲協方差反映傳感器讀數本身的可信度。當 R 相對于 P 很大時K 趨近于 0濾波器更相信模型預測當 R 相對于 P 很小時K 趨近于 1濾波器更相信傳感器。這就是數據融合用卡爾曼濾波的本質它不做固定權重的平均而是讓權重隨不確定度實時變化。某一時刻 GPS 信號抖動劇烈R 變大融合結果自然偏向里程計下一秒 GPS 恢復權重又自動切回來。Q 描述的是模型本身的不可信程度。如果系統實際在加速而狀態方程用的是勻速模型那么 Q 就要留出足夠的余量來容納這種未建模的動態Q 設得太小濾波器會過度信任預測表現為輸出平滑但滯后嚴重。Q 設得太大融合結果又會被過程噪聲淹沒噪聲抑制效果退化成一階低通濾波甚至更差。提示卡爾曼濾波和常見的一階低通濾波的差別就在這個 K 是自適應計算的。一階低通濾波的系數是常數卡爾曼的“系數”是隨 P 和 R 不斷更新的變量。2.3 定義融合系統的狀態向量與測量矩陣一張表說清維度對應以 datsfusion 里最常見的二維定位融合為例狀態向量一般取四個量x 方向位置、x 方向速度、y 方向位置、y 方向速度。GPS 輸出位置里程計輸出速度H 矩陣就要按這個順序去對齊。狀態向量成員物理含義對應傳感器測量方程中的體現p_xx 軸位置GPSz_gps p_x v_gpsv_xx 軸速度里程計/IMUz_odom v_x v_odomp_yy 軸位置GPSz_gps p_y v_gpsv_yy 軸速度里程計/IMUz_odom v_y v_odom狀態向量一旦含速度A 矩陣自然就是勻速模型的形式位置列里帶一個 dt速度列保持 1。很多初學者把狀態向量只定義為位置用兩個位置傳感器做融合也能跑通但融合結果沒有速度估計后續做目標跟蹤時就沒有預測能力每次都要等測量到了才能動。既然做的是數據融合能多狀態量就多狀態量這樣 Q 矩陣能把速度的變化過程也建模進去預測出來的軌跡才連貫。H 矩陣的維度是“測量數 × 狀態維數”。GPS 和里程計同時到達時測量向量是兩維H 是 2×4 的矩陣每一行對應一個傳感器的觀測方程。寫完第一步就是檢查矩陣維度能不能相乘這一步錯了后面代碼跑起來全是維度報錯但報錯信息往往會把你引向完全無關的地方。3. 用卡爾曼濾波做數據融合一個能直接跑的最小 Python 實現3.1 最小卡爾曼濾波類predict 與 update 兩個核心方法先寫一個一維的單變量卡爾曼濾波器類再看它怎么融合兩路傳感器。單變量場景里所有矩陣都退化成標量邏輯最直觀。import numpy as np class KalmanFilter1D: 一維卡爾曼濾波用于說明 predict / update 的完整流程 def __init__(self, q0.01, p01.0): self.q q # 過程噪聲方差模型未建模動態的余量 self.p p0 # 初始協方差給大一點可以加速收斂 self.x 0.0 # 初始狀態估計 def predict(self, dt1.0): # 先驗估計狀態不變協方差累加過程噪聲 self.p self.p self.q * dt def update(self, z, r): # z 是傳感器讀數r 是該傳感器的測量噪聲方差 k self.p / (self.p r) # 卡爾曼增益 self.x self.x k * (z - self.x) # 用新息修正狀態 self.p (1 - k) * self.p # 更新后協方差 return self.x這里卡爾曼增益 k 的表達式是一維化的形式K P / (P R)。當 R 遠大于 Pk 趨近于 0測量修正量很小當 R 遠小于 Pk 趨近于 1狀態幾乎完全跟隨測量。update 里的(z - self.x)是新息表示“測量值與當前預測的差值”卡爾曼濾波的全部修正動作都建立在這個差值上。r 參數按傳感器分別傳入這給了數據融合一個天然的結構同一個濾波器不同傳感器各自帶自己的噪聲方差。3.2 雙傳感器融合的最小場景GPS 與里程計同時輸出位置現在構造一個仿真場景目標真實位置是恒定值 5.0GPS 噪聲標準差 2.0里程計噪聲標準差 0.5。注意這里用標準差構造數據傳入濾波器時要把標準差平方成方差。np.random.seed(42) true_pos 5.0 n 100 # 兩路傳感器觀測同一位置噪聲水平差異很大 gps true_pos np.random.normal(0, 2.0, n) # GPS大噪聲 odom true_pos np.random.normal(0, 0.5, n) # 里程計小噪聲 kf KalmanFilter1D(q0.01, p01.0) states [] for t in range(n): kf.predict(dt1.0) kf.update(gps[t], r2.0**2) # GPS 的 R 為 4.0 kf.update(odom[t], r0.5**2) # 里程計 R 為 0.25 states.append(kf.x) # 對比純 GPS 均值誤差 vs 融合結果誤差 gps_err np.abs(np.mean(gps) - true_pos) fusion_err np.abs(states[-1] - true_pos) print(GPS 均值誤差: %.3f, 卡爾曼融合誤差: %.3f % (gps_err, fusion_err))同一時刻兩路測量到達可以連續調用兩次 update。第一次用 GPS 更新協方差 P 會縮小第二次用里程計更新是在 GPS 更新完的基礎上繼續修正。順序本身不影響最終結果因為卡爾曼遞推滿足結合律但每次 update 前不要忘記先 predict 一次。r 參數要按傳感器分別傳入GPS 的 R 是 4.0里程計是 0.25后者權重天然更大。最終融合誤差通常遠小于純 GPS 的均值誤差這就是把測量噪聲模型化之后得到的收益。注意R 傳方差還是標準差是新手最容易踩的坑。傳感器手冊給的一般是“精度 ±0.5m”這是標準差傳進公式時要平方成 0.25。不平方也能跑但濾波結果對噪聲的抑制程度會整體失真。3.3 單變量到多維融合的改動邊界同一套公式換矩陣維度一維場景跑通后二維多維融合的代碼結構不用變只把標量換成矩陣。狀態向量 x 變成 4×1A 變成 4×4P 變成 4×4H 按傳感器測量維度取對應的行。predict 里做F P F.T Qupdate 里先算S H P H.T R再算K P H.T np.linalg.inv(S)。建議在代碼里把矩陣維度寫清楚后先用單位矩陣做一次冒煙測試狀態向量全取 1觀測全取 1跑幾步看輸出是否發散。發散就逐項檢查 A、H 的維度配對而不是直接懷疑濾波公式。矩陣融合和多傳感器融合里 90% 的運行時錯誤都是維度錯位一維代碼很難暴露這類問題。4. 擴展卡爾曼濾波與目標跟蹤非線性融合場景的升級路徑4.1 為什么目標跟蹤里要用擴展卡爾曼濾波而不是線性卡爾曼線性卡爾曼的前提是狀態轉移和測量方程都是線性的。但目標跟蹤場景里傳感器輸出的常常不是位置坐標而是距離和角度比如雷達測出(r, θ)換算成直角坐標是px r * cos(θ), py r * sin(θ)。這個映射關系天然帶三角函數不是線性方程。強行用線性卡爾曼的結果是測量更新這一步的 H 矩陣沒法表達真實的映射融合輸出會在目標轉彎時出現明顯偏差。擴展卡爾曼濾波EKF的處理方式是對非線性函數做一階泰勒展開用雅可比矩陣代替原來的線性 H 矩陣。每一時刻的 H 都不同它依賴當前狀態估計值所以要在每次更新時重新計算。這個“重新算 H”的動作就是 EKF 比線性卡爾曼多出來的核心計算量。4.2 雷達測距測角場景的 EKF 預測與更新代碼狀態仍然取[px, py, vx, vy]過程模型繼續用勻速模型測量模型改成非線性函數并顯式寫出雅可比矩陣。import numpy as np def ekf_predict(x, P, dt, Q): # 勻速模型的狀態轉移矩陣 F np.array([ [1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1] ]) x F x P F P F.T Q return x, P def ekf_update(x, P, z, R): px, py, vx, vy x r np.sqrt(px**2 py**2) # 雅可比矩陣測量方程對狀態求偏導 H np.array([ [px / r, py / r, 0, 0], [-py / (r**2), px / (r**2), 0, 0] ]) # 預測測量值 z_pred np.array([r, np.arctan2(py, px)]) # 新息協方差、增益、狀態與協方差更新 S H P H.T R K P H.T np.linalg.inv(S) innovation z - z_pred # 角度新息要歸一化到 [-pi, pi]否則角度跳變會擊穿濾波器 innovation[1] (innovation[1] np.pi) % (2 * np.pi) - np.pi x x K innovation P (np.eye(4) - K H) P return x, P, innovation雅可比矩陣的第一行是距離 r 對位置分量的偏導第二行是角度 θ 對位置分量的偏導。速度分量在測量方程里不出現對應位置全為 0。角度新息的歸一化是目標跟蹤里必須寫的細節角度從 179 度跳到 -179 度數值上變化了 358 度但真實角度變化只有 2 度。不做歸一化這個虛假的大新息會把濾波器狀態直接沖偏。4.3 融合發散前看這三個指標新息、協方差對角線、誤差包絡EKF 跑起來之后不要只盯著最終輸出曲線看融合是否發散應該用三個量化指標預判。新息的均值應該接近 0方差應該接近 S 矩陣的對角線。如果新息長期偏向一側說明模型有系統偏差比如勻速模型跟不上轉彎目標如果新息方差遠大于 S 的預期說明 R 設得太小濾波器過度相信測量。P 矩陣對角線代表每個狀態量的估計方差理想情況下應該逐步收斂并保持穩定。P 持續增長不下降說明更新環節沒有起到修正作用通常是 H 算錯或者 R 設置過大P 快速收斂到極小值往往是過度自信后續一旦出現異常測量濾波器根本來不及反應。誤差包絡是實際測量殘差與理論方差的比值。工程上常用一個移動窗口統計實際新息的標準差與 S 的理論值做對比比值在 1.5 倍以內算正常超過 3 倍基本可以判定濾波器模型失真需要回頭檢查 Q 和 R 的匹配度。5. 數據融合現場先調這三個參數再用新息驗證濾波效果5.1 R 矩陣標定用靜態數據算方差別靠手感R 是唯一能通過實驗直接測出來的參數。做法是把傳感器放在靜止環境下采集 100 到 200 個數據點直接算方差。代碼就三行samples np.array([...]) # 靜止時采集的傳感器讀數 r np.var(samples) # 測量噪聲方差靜態標定得到的 R 是基線值實際運行中還要根據場景微調。比如 GPS 在城市峽谷里噪聲會顯著增大這時候 R 按靜態值給濾波器會把嚴重偏離的測量當成正常數據吸收進去。工程上的常見做法是做一個簡單的環境判斷當新息連續多步超過 3 倍標準差時臨時把 R 放大 5 到 10 倍等新息回歸正常再切回來。5.2 Q 矩陣調參從滯后到跟手的現場步驟Q 沒有直接測量手段通常靠效果反推。調參步驟可以按下面這張表執行現象原因判斷調整方向輸出平滑但滯后嚴重Q 偏小模型過度自信Q 增大 3~5 倍讓預測跟隨動態輸出抖動明顯噪聲沒壓住Q 偏大或 R 偏小先驗證 R再減小 Q新息長期不為零模型誤差或 Q 太小Q 增大或檢查狀態轉移方程P 收斂后不下降R 過大按標定值重新給定 R調整 Q 時建議按對角元素逐項調不要整體縮放。位置對應的 Q 和速度對應的 Q 物理意義不同整體縮放一次往往會把原來合理的部分也弄壞。拿目標跟蹤來說位置過程噪聲來自目標加速度的隨機變化速度過程噪聲還包含模型本身的截斷誤差兩項要給不同的數量級。5.3 新息序列驗證與在線噪聲協方差估計的小技巧調完 Q 和 R 后用一個移動窗口持續統計新息的均值和方差。均值應該在 0 附近波動窗口方差與 S 的理論值匹配。這里有一個實用的在線自適應技巧用滑動窗口估計實際新息方差sigma_hat如果它持續大于 S說明 R 被低估了用兩者的比值去修正 Rwindow_size 50 innovation_history [] # 每步更新后追加新息 innovation_history.append(innovation[0]) if len(innovation_history) window_size: innovation_history.pop(0) # 窗口內的實際新息標準差與理論 S 對比 sigma_hat np.std(innovation_history) if sigma_hat 2.0 * np.sqrt(S[0, 0]): R_adapted R_original * (sigma_hat / np.sqrt(S[0, 0]))**2這個比值修正法在噪聲緩變場景里非常有效能自動適應傳感器性能退化。但它對環境突變不敏感如果目標突然大幅加速新息會驟增這時更合適的做法是臨時增大 Q 而不是調整 R。判斷依據很簡單目標動態變了新息增大但協方差 P 正常傳感器壞了新息和 P 同步惡化。把這兩個信號區分開融合系統才算真正具備現場可維護性。本文還有配套的精品資源點擊獲取