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