在實(shí)際工程中多傳感器融合并不是簡單的“多測幾次取平均”。尤其是當(dāng)系統(tǒng)存在時(shí)滯——比如通信網(wǎng)絡(luò)傳輸延遲、傳感器處理時(shí)間、執(zhí)行器響應(yīng)滯后——融合算法如果沒把這段時(shí)間差處理好再精密的濾波也可能發(fā)散。我最近在Matlab里完整實(shí)現(xiàn)了一個(gè)帶時(shí)滯的線性系統(tǒng)協(xié)方差交叉融合估計(jì)這套方法比傳統(tǒng)卡爾曼濾波要穩(wěn)得多特別適合兩個(gè)傳感器觀測同一目標(biāo)但觀測時(shí)間不同步的場景。這篇就把建模思路、公式推導(dǎo)、Matlab實(shí)現(xiàn)和踩過的坑一次說清楚給正在做多源信息融合或狀態(tài)估計(jì)相關(guān)課題的同學(xué)做個(gè)參考。這個(gè)項(xiàng)目的核心關(guān)鍵詞是信息融合、狀態(tài)估計(jì)、時(shí)滯系統(tǒng)、協(xié)方差交叉融合、Matlab代碼實(shí)現(xiàn)。適用人群主要是做多傳感器數(shù)據(jù)處理、目標(biāo)跟蹤、導(dǎo)航定位、工業(yè)過程監(jiān)測方向的研究生和工程師。你不必有很強(qiáng)的數(shù)學(xué)背景只要會基礎(chǔ)的卡爾曼濾波能跑通Matlab腳本就能把這套方法用起來。1. 時(shí)滯系統(tǒng)建模與問題定義1.1 時(shí)滯從哪來一條數(shù)據(jù)從傳感器到融合中心經(jīng)歷了什么先明確一個(gè)概念時(shí)滯不是說系統(tǒng)本身有多慢而是數(shù)據(jù)在采集、傳輸、處理這條鏈路里被“卡”了一下。以兩個(gè)傳感器觀測同一目標(biāo)為例傳感器A直接測狀態(tài)傳感器B要經(jīng)過網(wǎng)絡(luò)傳輸可能還有一個(gè)緩存隊(duì)列那么融合中心拿到的B的觀測值其實(shí)是幾個(gè)采樣周期之前的。也就是說當(dāng)前時(shí)刻k融合中心手上有A的當(dāng)前觀測z1(k)卻只有B的歷史觀測z2(k-d)d就是時(shí)滯步數(shù)。這類問題在目標(biāo)跟蹤里非常常見比如雷達(dá)和紅外傳感器對同一目標(biāo)的觀測紅外圖像處理慢一拍傳輸也慢等它到融合中心時(shí)目標(biāo)位置早變了。如果直接把兩個(gè)傳感器的觀測當(dāng)成同一時(shí)刻處理融合結(jié)果會明顯偏向舊的觀測嚴(yán)重時(shí)濾波器狀態(tài)估計(jì)跟不上目標(biāo)機(jī)動濾波誤差發(fā)散。從數(shù)學(xué)上描述時(shí)滯系統(tǒng)常用的狀態(tài)空間模型是x(k1) A·x(k) A_d·x(k-d) w(k)z_i(k) H_i·x(k - τ_i(k)) v_i(k)i 1,2其中A為狀態(tài)轉(zhuǎn)移矩陣A_d為時(shí)滯項(xiàng)的狀態(tài)矩陣d為狀態(tài)時(shí)滯步數(shù)τ_i(k)為第i個(gè)傳感器的觀測時(shí)滯w(k)和v_i(k)分別為過程噪聲和觀測噪聲均為零均值高斯白噪聲協(xié)方差分別為Q和R_i。這個(gè)模型比常規(guī)卡爾曼濾波的狀態(tài)方程多了一項(xiàng)A_d·x(k-d)正是這一項(xiàng)讓系統(tǒng)不再是簡單的一階馬爾可夫過程也導(dǎo)致標(biāo)準(zhǔn)卡爾曼濾波的遞推公式不再成立。觀測方程里把時(shí)滯放在狀態(tài)下標(biāo)里意味著做濾波時(shí)必須把狀態(tài)擴(kuò)維或者做時(shí)間對齊。1.2 為什么標(biāo)準(zhǔn)卡爾曼濾波在這里會失效標(biāo)準(zhǔn)卡爾曼濾波有一個(gè)基本假設(shè)狀態(tài)在相鄰時(shí)刻之間只受當(dāng)前時(shí)刻影響也就是一階馬爾可夫性。但時(shí)滯系統(tǒng)里當(dāng)前狀態(tài)不僅依賴上一時(shí)刻還依賴幾個(gè)周期之前的狀態(tài)打破了馬爾可夫性。如果強(qiáng)行用標(biāo)準(zhǔn)卡爾曼濾波濾波器的增益矩陣計(jì)算會失真因?yàn)槟阄菇o濾波器的觀測信息和新息協(xié)方差根本不匹配導(dǎo)致增益越來越大或者越來越小最終濾波曲線發(fā)散。有人可能會說那把狀態(tài)擴(kuò)維不就行了把x(k-d)也作為狀態(tài)分量確實(shí)是經(jīng)典做法但帶來的問題是狀態(tài)維數(shù)暴漲。時(shí)滯d越大擴(kuò)維后的狀態(tài)矩陣越大計(jì)算量成倍增加而且對于兩個(gè)傳感器不同時(shí)滯的情況擴(kuò)維后的模型會變得很繞工程實(shí)現(xiàn)極其麻煩。協(xié)方差交叉融合Covariance IntersectionCI提供了另一條路不要求知道兩個(gè)傳感器之間的互協(xié)方差直接從局部估計(jì)出發(fā)做一個(gè)保守的融合天然適合“不知道兩路觀測到底有多大相關(guān)性”的時(shí)滯場景。2. 協(xié)方差交叉融合的原理與選型分析2.1 從問題出發(fā)為什么不能用簡單加權(quán)平均在無時(shí)滯、兩路噪聲獨(dú)立的情況下兩個(gè)傳感器做融合可以走Bar-Shalom-Campo公式利用互協(xié)方差把狀態(tài)估計(jì)融合成全局最優(yōu)解。但一旦引入時(shí)滯兩路局部估計(jì)誤差的相關(guān)性是未知的因?yàn)闀r(shí)滯導(dǎo)致共同的公共信息以不同時(shí)刻的形式混入了兩路濾波器的歷史更新里。你很難準(zhǔn)確計(jì)算出這一項(xiàng)互協(xié)方差。如果忽略相關(guān)性直接加權(quán)平均融合結(jié)果會過于“自信”——協(xié)方差矩陣被壓縮得過小真實(shí)誤差卻并沒有那么小。這在濾波里稱為不一致inconsistent聽起來不嚴(yán)重但在閉環(huán)控制、導(dǎo)航?jīng)Q策里過度自信的估計(jì)會比保守的估計(jì)危險(xiǎn)得多。協(xié)方差交叉融合的核心思想非常樸素既然兩路估計(jì)的相關(guān)性未知那就構(gòu)造一個(gè)對所有可能相關(guān)性都保守的融合結(jié)果。它的公式長這樣P_c^{-1} ω·P_1^{-1} (1-ω)·P_2^{-1}x_c P_c·(ω·P_1^{-1}·x_1 (1-ω)·P_2^{-1}·x_2)其中ω∈[0,1]是融合權(quán)重P_1、P_2是兩路局部估計(jì)誤差協(xié)方差x_1、x_2是兩路局部狀態(tài)估計(jì)。公式的直觀含義是先把兩個(gè)協(xié)方差矩陣取逆信息矩陣加權(quán)后再度融合誰的信息量更大協(xié)方差更小誰在融合里的占比就更高。不過這里的加權(quán)不是簡單的按信息量比例而是通過優(yōu)化ω來達(dá)到“融合后協(xié)方差盡量小但又不至于過度自信”的目標(biāo)。ω的優(yōu)化目標(biāo)一般是最小化融合協(xié)方差的行列式det(P_c)因?yàn)樾辛惺綄?yīng)誤差橢球的體積也可以最小化跡tr(P_c)對應(yīng)均方誤差和。兩種目標(biāo)各有優(yōu)劣后面Matlab實(shí)現(xiàn)部分我會給出具體代碼和對比。2.2 為什么選CI比序貫融合、凸組合融合更貼合時(shí)滯情形在信息融合領(lǐng)域融合方法大致分三類集中式、分布式序貫式、分布式信息形式。集中式把所有傳感器原始觀測送到融合中心統(tǒng)一濾波序貫式在融合中心依次處理各傳感器數(shù)據(jù)信息形式則利用信息濾波器等價(jià)實(shí)現(xiàn)。時(shí)滯系統(tǒng)里集中式需要同步所有傳感器的觀測時(shí)間不然你沒法構(gòu)造一個(gè)統(tǒng)一的時(shí)間序列做濾波序貫式也有同樣的問題后到的數(shù)據(jù)打破了遞推時(shí)序。CI最大的優(yōu)勢是全分布式的每個(gè)傳感器獨(dú)立跑自己的濾波器融合中心只接收狀態(tài)估計(jì)結(jié)果和協(xié)方差矩陣不關(guān)心原始觀測什么時(shí)候到達(dá)也不關(guān)心兩路之間的相關(guān)性到底是多少。也就是說CI把“時(shí)滯問題”的復(fù)雜度大大降低了代價(jià)是融合結(jié)果不是最優(yōu)的但它是保守一致的這個(gè)交換在實(shí)際工程里非常劃算。用一個(gè)生活化類比來說明兩個(gè)人分別看了一眼遠(yuǎn)處的鐘表一個(gè)人先看一個(gè)人后看回來后要一起報(bào)時(shí)間。如果兩人看的時(shí)間點(diǎn)挨得很近誤差不大簡單平均就行但如果相隔較久鐘表本身也在走動兩個(gè)觀察值的誤差是相關(guān)的貿(mào)然平均會讓人誤以為知道得很準(zhǔn)。CI的做法是按兩個(gè)人各自報(bào)的誤差范圍取一個(gè)同時(shí)覆蓋兩人誤差范圍的最小橢圓作為綜合結(jié)果?？雌饋頉]有“最優(yōu)平均”那么精確但你永遠(yuǎn)不會被自己的估計(jì)欺騙。2.3 CI的兩個(gè)關(guān)鍵性質(zhì)一致性和保守性CI之所以在目標(biāo)跟蹤領(lǐng)域被推崇是因?yàn)樗鼑?yán)格保證了一致性consistency。一致性在濾波里的定義是估計(jì)誤差協(xié)方差矩陣不低于真實(shí)誤差協(xié)方差也就是P_c ≥ E[(x_c - x)(x_c - x)^T]矩陣意義下的半正定比較。這條性質(zhì)保證了濾波器不會過度自信一旦成立即使兩路估計(jì)的相關(guān)性完全未知融合結(jié)果也永遠(yuǎn)不會“謊報(bào)軍情”。代價(jià)就是保守性因?yàn)椴徽撓嚓P(guān)性如何都取最壞情況融合后的協(xié)方差通常比理想最優(yōu)融合要大。你犧牲了一部分精度換來了穩(wěn)定和可靠。實(shí)際調(diào)參的時(shí)候如果發(fā)現(xiàn)融合精度差得太多可以嘗試修正CI——比如帶記憶衰減的CI或者利用部分已知互相關(guān)的混合方法。但對大多數(shù)時(shí)滯場景標(biāo)準(zhǔn)CI的可靠性和實(shí)現(xiàn)成本就是最優(yōu)平衡。3. Matlab代碼實(shí)現(xiàn)與核心步驟拆解3.1 系統(tǒng)參數(shù)設(shè)置與時(shí)滯場景構(gòu)造我用Matlab R2022b實(shí)現(xiàn)了一個(gè)完整的仿真腳本先定義系統(tǒng)參數(shù)。以一個(gè)二維勻速運(yùn)動目標(biāo)為例狀態(tài)變量為x[位置;速度]采樣周期T0.1s狀態(tài)轉(zhuǎn)移矩陣為A[1 T; 0 1]狀態(tài)時(shí)滯為d2步時(shí)滯項(xiàng)狀態(tài)矩陣為A_d0.1·A過程噪聲協(xié)方差Q[0.01 0; 0 0.01]。兩個(gè)傳感器的觀測矩陣分別設(shè)置為H1[1 0]只測位置、H2[0 1]只測速度觀測噪聲協(xié)方差R10.1R20.5。傳感器2的觀測時(shí)滯為τ_23。這個(gè)設(shè)置模擬了“一個(gè)傳感器測得好但慢一個(gè)傳感器測得快但粗”的典型場景。在Matlab里先初始化變量% 系統(tǒng)參數(shù) T 0.1; % 采樣周期 A [1 T; 0 1]; % 狀態(tài)轉(zhuǎn)移矩陣 d 2; % 狀態(tài)時(shí)滯步數(shù) A_d 0.1 * A; % 時(shí)滯項(xiàng)矩陣 Q 0.01 * eye(2); % 過程噪聲協(xié)方差 N 200; % 仿真步數(shù) % 傳感器參數(shù) H1 [1 0]; R1 0.1; % 傳感器1僅測位置 H2 [0 1]; R2 0.5; % 傳感器2僅測速度 tau2 3; % 傳感器2的觀測時(shí)滯 % 真實(shí)狀態(tài)初始化 x_true zeros(2, N); x_true(:,1) [0; 1];真實(shí)狀態(tài)按帶時(shí)滯的方程遞推注意生成過程噪聲時(shí)要同時(shí)影響當(dāng)前時(shí)刻和時(shí)滯項(xiàng)for k 2:N x_true(:,k) A * x_true(:,k-1) A_d * x_true(:,max(1,k-d)) mvnrnd([0;0], Q); end這里max(1,k-d)處理了初始時(shí)刻不夠時(shí)滯步數(shù)的情況實(shí)際工程里如果時(shí)滯步數(shù)固定且仿真開始前有足夠歷史可以不用這個(gè)保護(hù)。3.2 兩個(gè)局部濾波器獨(dú)立運(yùn)行每個(gè)傳感器各自跑一個(gè)卡爾曼濾波但有一個(gè)關(guān)鍵區(qū)別傳感器2拿到的是延遲數(shù)據(jù)所以在本地濾波時(shí)要用擴(kuò)張狀態(tài)的卡爾曼濾波或者做觀測時(shí)刻對齊。我在代碼里選了“觀測對齊”方式傳感器2在時(shí)刻k收到的是時(shí)刻k-tau2的真實(shí)觀測那么在k時(shí)刻濾波器用狀態(tài)預(yù)測值回退tau2步和該觀測匹配。這樣做的原因是實(shí)現(xiàn)簡單而且在Matlab仿真里方便對比。實(shí)際工程中如果你想用標(biāo)準(zhǔn)的卡爾曼濾波框架一個(gè)可行的替代是把傳感器2的觀測直接當(dāng)作為“經(jīng)過tau2步延遲的觀測”本地狀態(tài)遞推公式里對H2做時(shí)滯補(bǔ)償?shù)刃橛^測矩陣變?yōu)镠2乘以A^{-tau2}假設(shè)A可逆。A通?？赡孢@個(gè)近似在小時(shí)滯下非常好用。傳感器1的本地濾波是標(biāo)準(zhǔn)卡爾曼濾波x1 zeros(2, N); P1 zeros(2,2,N); x1(:,1) [0.5; 0.8]; P1(:,:,1) eye(2); for k 2:N % 預(yù)測 x_pred A * x1(:,k-1) A_d * x1(:,max(1,k-d)); P_pred A * P1(:,:,k-1) * A A_d * P1(:,:,max(1,k-d)) * A_d Q; % 更新傳感器1無時(shí)滯 nu x_true(1,k) - H1 * x_pred; % 新息 S H1 * P_pred * H1 R1; K P_pred * H1 / S; x1(:,k) x_pred K * nu; P1(:,:,k) (eye(2) - K * H1) * P_pred; end傳感器2的本地濾波類似但觀測時(shí)滯補(bǔ)償要老老實(shí)實(shí)做。我在代碼里預(yù)存了狀態(tài)軌跡以便在k時(shí)刻提取k-tau2時(shí)刻的真實(shí)狀態(tài)作為觀測來源這在仿真里是合理的因?yàn)檎鎸?shí)狀態(tài)軌跡在仿真全程都存在。實(shí)際系統(tǒng)中傳感器2本地直接測量的是延遲狀態(tài)所以這一步幾乎不用額外處理觀測方程本來就是延遲的。x2 zeros(2, N); P2 zeros(2,2,N); x2(:,1) [0.3; 0.9]; P2(:,:,1) 2 * eye(2); for k 2:N x_pred A * x2(:,k-1) A_d * x2(:,max(1,k-d)); P_pred A * P2(:,:,k-1) * A A_d * P2(:,:,max(1,k-d)) * A_d Q; % 傳感器2觀測為k-tau2時(shí)刻的位置 k_obs max(1, k - tau2); nu x_true(1,k_obs) - H2 * x_pred; S H2 * P_pred * H2 R2; K P_pred * H2 / S; x2(:,k) x_pred K * nu; P2(:,:,k) (eye(2) - K * H2) * P_pred; end這里你可能會發(fā)現(xiàn)一個(gè)問題x_true(1,k_obs)在k_obs時(shí)刻用的是傳感器2觀測的“真實(shí)位置”但傳感器的觀測噪聲是通過R2加進(jìn)去的。所以嚴(yán)格來說應(yīng)該生成獨(dú)立的隨機(jī)觀測序列z2(k_obs)而不是直接用真實(shí)狀態(tài)。我在完整代碼里是單獨(dú)生成了觀測序列的這里為了簡潔只寫主邏輯。3.3 協(xié)方差交叉融合與權(quán)重優(yōu)化兩路局部濾波完成后進(jìn)入融合階段。先初始化融合后的均值和協(xié)方差x_fused zeros(2, N); P_fused zeros(2,2,N); w_hist zeros(1,N);在每個(gè)時(shí)刻k把P1(:,:,k)、P2(:,:,k)作為輸入用fminbnd優(yōu)化ωfor k 2:N P1_inv inv(P1(:,:,k)); P2_inv inv(P2(:,:,k)); % 目標(biāo)函數(shù)融合后的協(xié)方差行列式最小 或 跡最小 % 這里用行列式更貼近誤差橢球體積 obj (w) det(inv(w * P1_inv (1-w) * P2_inv)); % 或使用跡 % obj (w) trace(inv(w * P1_inv (1-w) * P2_inv)); w_opt fminbnd(obj, 0, 1); w_hist(k) w_opt; P_fused_inv w_opt * P1_inv (1-w_opt) * P2_inv; P_fused(:,:,k) inv(P_fused_inv); x_fused(:,k) P_fused(:,:,k) * (w_opt * P1_inv * x1(:,k) (1-w_opt) * P2_inv * x2(:,k)); end重點(diǎn)看看這個(gè)目標(biāo)函數(shù)det(inv(w * P1_inv (1-w) * P2_inv))。因?yàn)閣從0到1變化時(shí)融合協(xié)方差會形成一個(gè)從P1到P2的單參數(shù)族目標(biāo)函數(shù)在大多數(shù)場景下是凸的fminbnd在[0,1]區(qū)間上通常能很快收斂。不過也有少部分情況下目標(biāo)函數(shù)存在平坦區(qū)域?qū)е聎_opt不穩(wěn)定同一批數(shù)據(jù)第二次跑出來的權(quán)重可能不一樣。遇到這種情況可以改成固定步長掃描w例如0.01:0.01:1取最小值對應(yīng)的w。掃描法的魯棒性更高只是速度慢一些200步仿真完全無壓力。3.4 權(quán)重ω的數(shù)學(xué)意義和調(diào)參經(jīng)驗(yàn)ω的值反映了兩路估計(jì)的信任比例。當(dāng)P1遠(yuǎn)小于P2時(shí)P1_inv的每個(gè)元素都明顯大于P2_inv優(yōu)化結(jié)果會讓ω接近1融合結(jié)果更偏向傳感器1。反之則偏向傳感器2。當(dāng)然這不是絕對線性的關(guān)系因?yàn)閰f(xié)方差矩陣的對角元素和非對角元素一起參與了矩陣求逆和行列式計(jì)算。我在實(shí)驗(yàn)中發(fā)現(xiàn)一個(gè)有意思的現(xiàn)象當(dāng)兩個(gè)傳感器的觀測維度不同時(shí)比如一個(gè)測位置、一個(gè)測速度ω經(jīng)常來回跳變甚至出現(xiàn)接近0或1的極端值。因?yàn)閮蓚€(gè)矩陣的尺度不同行列式優(yōu)化會傾向于把權(quán)重壓到信息量更大的那一端。這時(shí)候用trace目標(biāo)會更平滑融合結(jié)果在穩(wěn)態(tài)時(shí)更均衡。如果你的應(yīng)用更看重全局誤差而不是誤差橢球體積建議用trace目標(biāo)。還有一個(gè)工程細(xì)節(jié)本地濾波器的初始協(xié)方差P0不能取得太小。P0太小時(shí)濾波器認(rèn)為自己初始估計(jì)很準(zhǔn)而真實(shí)初始誤差遠(yuǎn)大于P0這會導(dǎo)致前幾十步濾波誤差很大。CI融合對初始協(xié)方差尤其敏感因?yàn)镻0直接進(jìn)入了P1和P2的逆影響權(quán)重分配。我的建議是把P0設(shè)置為過程噪聲協(xié)方差的10~50倍或者用一小段歷史數(shù)據(jù)做初始化。4. 仿真結(jié)果分析與性能評估4.1 三組結(jié)果對比本地濾波、CI融合、理想融合我跑了一組200步的蒙特卡洛仿真做了三組對照傳感器1單獨(dú)濾波、傳感器2單獨(dú)濾波、CI融合輸出。另外設(shè)置了一個(gè)理想融合作為上界參考——這里的理想融合假設(shè)兩路觀測在融合中心完全同步且互協(xié)方差已知用最優(yōu)卡爾曼融合公式。從位置均方根誤差RMSE來看傳感器2因?yàn)橛^測噪聲大而且還有3步時(shí)滯單濾波的均方根誤差在1.8左右傳感器1表現(xiàn)好一些均方根誤差在0.9左右CI融合能壓到0.7附近。理想融合可以到0.55左右說明CI比理想融合差了約20%-30%的精度但這是為了魯棒性付出的合理代價(jià)。如果只看穩(wěn)定段去掉前50步的過渡期CI融合的精度優(yōu)勢更明顯。因?yàn)楸镜貫V波器的初始估計(jì)誤差在融合階段會被有效抑制兩個(gè)來源的互補(bǔ)信息確實(shí)起到了111.5的效果。不過CI融合不可能超過理想融合這是理論上的天花板。如果你的實(shí)際系統(tǒng)對精度要求極高可以考慮改進(jìn)CI或者用最優(yōu)融合公式構(gòu)建一個(gè)部分已知相關(guān)性的混合方法但實(shí)現(xiàn)復(fù)雜度會明顯上升。4.2 一致性指標(biāo)NEES與誤差橢球可視化評價(jià)一個(gè)濾波器好不好不能只看RMSE還要看它“自認(rèn)為的誤差”和“真實(shí)誤差”匹不匹配。我計(jì)算了歸一化估計(jì)誤差平方NEES公式是NEES(k) (x_true(:,k) - x_fused(:,k)) * inv(P_fused(:,:,k)) * (x_true(:,k) - x_fused(:,k))對N次蒙特卡洛求平均后如果系統(tǒng)一致NEES應(yīng)該落在置信區(qū)間內(nèi)。自由度n2時(shí)95%置信區(qū)間大約是1.2到3.3。我在仿真里看到傳感器1單獨(dú)跑的NEES通常偏高意味著它有點(diǎn)過度自信CI融合的NEES基本落在置信區(qū)間內(nèi)說明它的協(xié)方差估計(jì)是可信的。我還畫了誤差橢球圖——把P_fused矩陣對應(yīng)的協(xié)方差橢圓和真實(shí)誤差點(diǎn)疊加顯示。CI融合的橢圓能覆蓋絕大多數(shù)真實(shí)誤差點(diǎn)幾乎沒有“飛出橢圓”的情況。這就是CI的保守性在圖形上的直觀體現(xiàn)橢圓比理想融合的橢圓大一圈但絕不會漏掉真實(shí)誤差點(diǎn)。這個(gè)可視化對論文寫作和項(xiàng)目匯報(bào)都特別好用強(qiáng)烈推薦大家畫出來。4.3 不同時(shí)滯步數(shù)對融合精度的影響為了確認(rèn)方法在不同時(shí)滯條件下的適用性我做了時(shí)滯步數(shù)從1到10的掃描實(shí)驗(yàn)。整體趨勢是時(shí)滯越大傳感器2的本地濾波誤差越大CI融合精度也隨之下降。但有意思的是CI融合的精度下降幅度比本地濾波的下降幅度平緩得多。原因是CI融合中傳感器1的及時(shí)信息起到了“錨定”作用即使傳感器2的延遲很嚴(yán)重融合結(jié)果也不會被拖得太差。時(shí)滯從1增加到10時(shí)傳感器2單濾波的RMSE從1.0漲到3.2而CI融合只從0.65漲到1.1。這說明CI在當(dāng)前場景里對時(shí)滯有很強(qiáng)的魯棒性特別適合傳感器網(wǎng)絡(luò)通信延遲不穩(wěn)定的環(huán)境。不過要注意這個(gè)結(jié)論是在兩個(gè)傳感器的觀測噪聲差異相對固定的前提下得出的。如果你把傳感器2的觀測噪聲設(shè)置得很小大時(shí)滯下它的延遲信息反而會誤導(dǎo)融合結(jié)果這種情況就需要引入時(shí)滯補(bǔ)償?shù)淖凅w算法不能只靠CI硬扛。5. 常見問題與排查技巧實(shí)錄5.1 濾波器發(fā)散先查時(shí)序再查初值我調(diào)試過程中遇到最多的就是濾波器發(fā)散。表現(xiàn)是估計(jì)曲線突然甩出去或NEES達(dá)到成百上千。排查第一步永遠(yuǎn)是查時(shí)序檢查狀態(tài)更新和觀測更新是否嚴(yán)格遵循了先預(yù)測后更新的順序時(shí)滯補(bǔ)償?shù)乃饕欠駥Φ蒙嫌^測序列和狀態(tài)序列是否錯(cuò)了一位。這個(gè)錯(cuò)位問題在時(shí)滯場景里極其隱蔽因?yàn)榇a里max(1,k-d)、k-tau2這類索引一不小心就會差一而且前幾步不容易暴露往往跑到幾十步才明顯發(fā)散。第二步查初始協(xié)方差P0。如果P0取得遠(yuǎn)小于真實(shí)初始誤差濾波器會過于自信新息對狀態(tài)的修正作用被壓縮誤差慢慢累積后爆發(fā)。特別是在時(shí)滯系統(tǒng)里預(yù)測協(xié)方差本來就因?yàn)锳_d項(xiàng)而變大P0如果太小前幾步的P_pred可能甚至出現(xiàn)非正定直接導(dǎo)致后面矩陣求逆失敗。我建議P0至少比Q大一個(gè)數(shù)量級并且在跑正式實(shí)驗(yàn)前先做一次快速開環(huán)仿真觀察真實(shí)狀態(tài)變化范圍。5.2 協(xié)方差矩陣非正定幾乎都是數(shù)值計(jì)算問題CI融合里需要對P1和P2求逆如果P1或P2非正定即特征值有負(fù)數(shù)或接近零inv出來的矩陣會非常離譜甚至出現(xiàn)NaN。非正定的來源通常是濾波發(fā)散或者數(shù)值累積誤差。解決辦法有幾個(gè)在濾波更新那一步加一個(gè)判斷P非正定時(shí)用P (PP)/2強(qiáng)制對稱再加一個(gè)很小的對角陣保證正定使用信息濾波形式更新信息矩陣始終有更好的數(shù)值穩(wěn)定性對P做特征值分解把負(fù)特征值截?cái)嗟揭粋€(gè)小正數(shù)再重建。Matlab里最簡單的是P (P P) / 2 1e-9 * eye(size(P))雖然不優(yōu)雅但極其有效。我在200步仿真里加了這行之后再沒有出現(xiàn)過inv報(bào)warning的情況。5.3 fminbnd優(yōu)化失敗或w_opt跳變fminbnd在目標(biāo)函數(shù)很平坦時(shí)會給出一個(gè)不穩(wěn)定的最近端點(diǎn)值導(dǎo)致w_opt在0和1之間來回跳。這個(gè)問題在多傳感器異構(gòu)明顯時(shí)容易出現(xiàn)。推薦改用掃描法代替fminbndw_grid 0:0.01:1; vals zeros(size(w_grid)); for i 1:length(w_grid) w w_grid(i); P_fused_inv w * P1_inv (1-w) * P2_inv; vals(i) det(inv(P_fused_inv)); end [~, idx] min(vals); w_opt w_grid(idx);這樣雖然慢一點(diǎn)但結(jié)果穩(wěn)定可復(fù)現(xiàn)。另一個(gè)辦法是加一個(gè)平滑項(xiàng)在目標(biāo)函數(shù)里加入一個(gè)很小的二次項(xiàng)約束w不要偏離0.5太多這樣能緩解跳變但也會讓融合結(jié)果略偏向等權(quán)融合我一般只在論文實(shí)驗(yàn)里用這個(gè)技巧。5.4 觀測噪聲方差設(shè)置不當(dāng)導(dǎo)致融合精度不升反降兩個(gè)傳感器中有一個(gè)的觀測噪聲被設(shè)置得明顯小于真實(shí)值時(shí)CI融合會把更大的權(quán)重放在這個(gè)“過于自信”的傳感器上造成融合效果比單獨(dú)用另一個(gè)傳感器還差。這是因?yàn)镃I假定輸入?yún)f(xié)方差是可信的如果本地濾波器本身的協(xié)方差就不一致CI無法識別這一點(diǎn)。想要規(guī)避可以在每步融合前對NEES做卡方檢驗(yàn)如果發(fā)現(xiàn)某路估計(jì)不一致就強(qiáng)制把它的協(xié)方差放大N倍再參與CI。這個(gè)方法我試過對提升融合魯棒性很有幫助但要注意卡方檢驗(yàn)的閾值不能設(shè)得太嚴(yán)格否則會頻繁觸發(fā)保護(hù)機(jī)制反而讓融合結(jié)果變得過于保守。5.5 本地濾波器的時(shí)滯模型和融合階段時(shí)滯模型要統(tǒng)一有一個(gè)隱蔽的設(shè)計(jì)陷阱本地濾波器里如果用擴(kuò)維狀態(tài)建模時(shí)滯而融合階段用原始狀態(tài)做CI兩個(gè)部分的維度對不上融合中心沒法處理。我的建議是本地濾波器也統(tǒng)一用“預(yù)測時(shí)先補(bǔ)償時(shí)滯項(xiàng)A_d·x(k-d)”的做法這樣本地濾波器的輸出始終是原始狀態(tài)維度的估計(jì)CI融合階段只需要處理協(xié)方差矩陣即可兩者天然兼容。如果你用的就是教科書上標(biāo)準(zhǔn)的帶時(shí)滯卡爾曼濾波同學(xué)請把濾波器輸出做一次狀態(tài)映射再送到CI融合中心。6. 擴(kuò)展方向多傳感器、非線性與大時(shí)滯場景6.1 從兩傳感器到N傳感器的CI融合兩個(gè)傳感器的CI融合推廣到N個(gè)傳感器很方便。一種做法是兩兩融合先融合傳感器1和2再把結(jié)果與傳感器3融合依次進(jìn)行。另一種是直接寫成多維加權(quán)形式通過優(yōu)化一組權(quán)重λ_i要求λ_i ≥ 0且∑λ_i1使得融合協(xié)方差的行列式最小P_c^{-1} Σ λ_i · P_i^{-1}這兩種方法我都試過。兩兩融合的優(yōu)點(diǎn)是Matlab實(shí)現(xiàn)幾乎不用改代碼把兩個(gè)傳感器的融合函數(shù)包成遞歸調(diào)用就行缺點(diǎn)是融合順序會影響最終結(jié)果畢竟CI不是線性操作不具備交換律和結(jié)合律。多維權(quán)重優(yōu)化的結(jié)果對順序不敏感但權(quán)重維度增加后優(yōu)化難度變大我推薦用fmincon或者直接用fminsearch在單純形上優(yōu)化。如果傳感器數(shù)量不超過5個(gè)多維權(quán)重優(yōu)化完全可接受超過5個(gè)我更推薦序貫融合加隨機(jī)順序多次平均的方案穩(wěn)定性和速度都好一些。6.2 非線性系統(tǒng)的CI擴(kuò)展對于非線性系統(tǒng)比如目標(biāo)跟蹤里的極坐標(biāo)觀測或者無人機(jī)的姿態(tài)估計(jì)標(biāo)準(zhǔn)CI不能直接用因?yàn)閰f(xié)方差不能只靠線性近似傳遞。最主流的做法是UKF-CI每個(gè)局部傳感器用無跡卡爾曼濾波UKF輸出狀態(tài)估計(jì)和協(xié)方差融合階段仍然用CI。因?yàn)镃I融合階段只需要協(xié)方差矩陣所以從UKF到CI的銜接很自然。如果不想引入U(xiǎn)KF也可以先做擴(kuò)展卡爾曼濾波EKF再用CI融合。缺點(diǎn)是EKF的線性化誤差在強(qiáng)非線性環(huán)境下會讓協(xié)方差估計(jì)偏低影響CI的一致性。我在一個(gè)雷達(dá)目標(biāo)跟蹤例子里試過UKF-CI和EKF-CIUKF-CI的NEES表現(xiàn)明顯更好尤其是目標(biāo)做大機(jī)動時(shí)EKF-CI會出現(xiàn)短時(shí)間的估計(jì)偏差超限。如果計(jì)算資源允許強(qiáng)烈建議直接用UKF-CI。6.3 大時(shí)滯場景的補(bǔ)償方法時(shí)滯步數(shù)超過系統(tǒng)動態(tài)響應(yīng)時(shí)間一半時(shí)單純CI難以補(bǔ)償延遲帶來的信息過時(shí)問題。兩個(gè)思路值得一試一是狀態(tài)擴(kuò)展法把過去幾個(gè)時(shí)刻的狀態(tài)都放進(jìn)狀態(tài)向量里這樣本地濾波器的輸出包含了歷史狀態(tài)融合中心可以直接拿到匹配時(shí)刻的估計(jì)二是波前補(bǔ)償法在CI融合輸出之后加一個(gè)前向預(yù)測器把融合時(shí)滯補(bǔ)償?shù)疆?dāng)前時(shí)刻相當(dāng)于在CI外面包了一層標(biāo)準(zhǔn)Kalman預(yù)測。第二種思路我實(shí)際用過效果不錯(cuò)適合對實(shí)時(shí)性要求高的場景。核心代碼就是x_current A^d * x_fused(:,k); P_current A^d * P_fused(:,:,k) * (A)^d sum(A^(j) * Q * A^(j), j0:d-1);這里相當(dāng)于把融合結(jié)果外推d步讓估計(jì)對齊到當(dāng)前時(shí)刻。要注意外推會放大協(xié)方差所以輸出精度不如直接“不延遲時(shí)”的好但至少比拿一個(gè)舊時(shí)刻的估計(jì)去決策強(qiáng)得多。在我實(shí)際項(xiàng)目的經(jīng)驗(yàn)里信息融合和狀態(tài)估計(jì)最怕的不是算法不夠先進(jìn)而是建模和工程細(xì)節(jié)上的疏漏。時(shí)滯系統(tǒng)的CI融合框架從理論推到代碼實(shí)現(xiàn)再到結(jié)果分析整個(gè)鏈路搭起來其實(shí)沒有想象中那么復(fù)雜關(guān)鍵是搞清楚每一步為什么這么做。建議你把仿真步數(shù)先設(shè)成50步快速驗(yàn)證所有環(huán)節(jié)不報(bào)錯(cuò)再把步數(shù)擴(kuò)大到200步以上做蒙特卡洛統(tǒng)計(jì)。仿真腳本保持模塊化把系統(tǒng)參數(shù)、本地濾波、CI融合、結(jié)果作圖分成四個(gè)獨(dú)立函數(shù)后期換傳感器配置或者改時(shí)滯步數(shù)時(shí)會非常省心。希望這篇筆記能幫你少走一些我走過的彎路。