1. 為什么無(wú)人機(jī)追蹤無(wú)人車需要自適應(yīng)濾波做無(wú)人機(jī)目標(biāo)跟蹤的項(xiàng)目時(shí)最常遇到的一個(gè)尷尬是你明明用著教科書(shū)里標(biāo)準(zhǔn)的卡爾曼濾波器代碼也沒(méi)寫(xiě)錯(cuò)但跑真實(shí)數(shù)據(jù)時(shí)就是會(huì)出現(xiàn)目標(biāo)明明在轉(zhuǎn)彎濾波器還在直線外推或者GPS噪聲突然變大估計(jì)軌跡一下就飄了的情況。我當(dāng)時(shí)在實(shí)現(xiàn)無(wú)人機(jī)對(duì)地面無(wú)人車的持續(xù)追蹤時(shí)就是在這樣的問(wèn)題里反復(fù)折騰了很久最后把固定噪聲參數(shù)換成了自適應(yīng)估計(jì)效果才穩(wěn)定下來(lái)。這個(gè)任務(wù)本身很典型一架飛行中的無(wú)人機(jī)向下觀察一輛在地面運(yùn)動(dòng)的無(wú)人車需要實(shí)時(shí)估計(jì)它的位置、速度甚至加速度輸出給飛行控制器保持跟蹤。難點(diǎn)在于地面車的運(yùn)動(dòng)模式不是恒定的——它可能直線行駛、突然轉(zhuǎn)彎、原地繞圈甚至急停急走而無(wú)人機(jī)的測(cè)量數(shù)據(jù)也有很大的不確定性視覺(jué)定位在光照變化下會(huì)漂移定位傳感器在建筑物遮擋下會(huì)出現(xiàn)明顯的野值噪聲。傳統(tǒng)的卡爾曼濾波器假設(shè)系統(tǒng)噪聲和測(cè)量噪聲的協(xié)方差矩陣 Q 和 R 是已知常量這在模型匹配、噪聲平穩(wěn)的情況下表現(xiàn)不錯(cuò)但真實(shí)場(chǎng)景不滿足這個(gè)假設(shè)濾波器就會(huì)失靈。自適應(yīng)卡爾曼濾波器的核心就是在濾波過(guò)程中根據(jù)實(shí)際觀測(cè)數(shù)據(jù)反過(guò)來(lái)估計(jì)當(dāng)前的噪聲統(tǒng)計(jì)特性讓濾波器增益動(dòng)態(tài)匹配真實(shí)的測(cè)量可靠度。說(shuō)白了就是讓濾波器自己知道現(xiàn)在測(cè)量數(shù)據(jù)靠譜不靠譜。如果靠譜就多相信觀測(cè)如果不靠譜就多相信運(yùn)動(dòng)模型預(yù)測(cè)。這個(gè)思想并不復(fù)雜但工程實(shí)現(xiàn)有很多細(xì)節(jié)稍不注意容易弄巧成拙。這篇文章我就把我在無(wú)人機(jī)追蹤無(wú)人車這個(gè)場(chǎng)景中從建模到MATLAB實(shí)現(xiàn)再到調(diào)參踩坑的完整過(guò)程分享出來(lái)重點(diǎn)講自適應(yīng)部分怎么落地以及最終怎么和純固定參數(shù)的卡爾曼濾波器做對(duì)比驗(yàn)證。這套內(nèi)容適合正在做目標(biāo)跟蹤、多傳感器融合或者要跑無(wú)人機(jī)仿真實(shí)驗(yàn)的開(kāi)發(fā)者。如果你已經(jīng)會(huì)用基礎(chǔ)卡爾曼濾波只是想提升在復(fù)雜場(chǎng)景下的魯棒性那這篇文章可以直接幫你跳過(guò)不少?gòu)澛?。如果你剛開(kāi)始接觸濾波我也把狀態(tài)空間建模和推導(dǎo)放到了第二節(jié)保證你讀起來(lái)不吃力。2. 系統(tǒng)模型搭建把追蹤問(wèn)題變成狀態(tài)估計(jì)問(wèn)題2.1 狀態(tài)向量不能只存位置速度估計(jì)同樣關(guān)鍵我把無(wú)人車當(dāng)作一個(gè)在地平面上運(yùn)動(dòng)的質(zhì)點(diǎn)無(wú)人機(jī)在空中懸停或勻速巡航主要關(guān)心的是地面車在水平二維平面上的運(yùn)動(dòng)狀態(tài)。這里有一個(gè)常見(jiàn)誤區(qū)很多人只把位置放進(jìn)狀態(tài)向量比如 x [px, py]然后直接用位置觀測(cè)。這樣確實(shí)能濾波但延遲很大因?yàn)闉V波器沒(méi)有內(nèi)部模型去推算速度位置變化完全靠觀測(cè)去拉。我一開(kāi)始也這么干過(guò)結(jié)果車勻速跑的時(shí)候估計(jì)軌跡總落后一大截后來(lái)把速度加進(jìn)狀態(tài)就立刻好了。完整的狀態(tài)向量我定義為x [px, py, vx, vy]^T其中 px, py 是地面車在水平坐標(biāo)系的坐標(biāo)vx, vy 是相應(yīng)速度分量。這里不把無(wú)人機(jī)的姿態(tài)和高度放進(jìn)狀態(tài)里因?yàn)槟鞘秋w行控制的問(wèn)題追蹤任務(wù)里我默認(rèn)無(wú)人機(jī)自身定位是已知的只估計(jì)目標(biāo)的相對(duì)運(yùn)動(dòng)。如果你在工程中需要將觀測(cè)轉(zhuǎn)換到無(wú)人機(jī)機(jī)體坐標(biāo)系可以在觀測(cè)方程里加一個(gè)坐標(biāo)變換矩陣這屬于后續(xù)擴(kuò)展不在這篇文章的核心范圍內(nèi)。2.2 運(yùn)動(dòng)模型怎么選CV模型在多數(shù)場(chǎng)景夠用地面無(wú)人車在短時(shí)間段內(nèi)的運(yùn)動(dòng)用常速度模型Constant Velocity簡(jiǎn)稱CV是一個(gè)很實(shí)用的折中。它假設(shè)目標(biāo)速度在采樣間隔內(nèi)基本不變但留有一定隨機(jī)擾動(dòng)用來(lái)吸收加速度變化。狀態(tài)轉(zhuǎn)移矩陣F [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]過(guò)程噪聲表現(xiàn)為加速度擾動(dòng)?？梢杂靡粋€(gè)高斯白噪聲向量 [ax, ay]^T 表示兩個(gè)方向上的加速度噪聲那么過(guò)程噪聲輸入矩陣 G 和協(xié)方差 Q 可以寫(xiě)成G [0.5dt^2 0; 0 0.5dt^2; dt 0; 0 dt]Q G * [sa^2, 0; 0, sa^2] * G^T其中 sa 是加速度擾動(dòng)的標(biāo)準(zhǔn)差可以理解成目標(biāo)有多不聽(tīng)話。對(duì)于無(wú)人車我一般取 sa 在 0.1 到 0.5 m/s^2 之間。你也可以用更高階的勻加速模型CA加入加速度項(xiàng)但會(huì)帶來(lái)兩個(gè)問(wèn)題狀態(tài)維數(shù)增加導(dǎo)致計(jì)算量上升而且如果目標(biāo)實(shí)際沒(méi)有顯著加速度CA模型會(huì)讓濾波器更容易受噪聲干擾。CV模型在大部分追蹤場(chǎng)景下已經(jīng)是夠用且穩(wěn)的選擇。2.3 觀測(cè)方程定位模塊直接給位置觀測(cè)無(wú)人機(jī)獲取地面車位置最常見(jiàn)的方式是機(jī)載定位模塊比如GPS/RTK結(jié)合視覺(jué)識(shí)別框的位置映射。在仿真里我直接用位置觀測(cè)z [px_meas; py_meas]觀測(cè)矩陣H [1 0 0 0; 0 1 0 0]測(cè)量噪聲協(xié)方差 R 是一個(gè) 2x2 矩陣通常假設(shè)兩個(gè)方向獨(dú)立即 R [rx, 0; 0, ry]?，F(xiàn)實(shí)中 rx 和 ry 可能不相等比如飛行方向上的位置誤差小、垂直方向誤差大所以我建議先按兩個(gè)獨(dú)立標(biāo)準(zhǔn)差來(lái)設(shè)置。2.4 卡爾曼濾波的五步方程先統(tǒng)一符號(hào)后面好講為了后面代碼完全對(duì)得上我把標(biāo)準(zhǔn)卡爾曼濾波的計(jì)算步驟列在這里后面所有自適應(yīng)改進(jìn)都基于這套框架預(yù)測(cè) x_pred F * x_est P_pred F * P_est * F^T Q更新 y z - H * x_pred S H * P_pred * H^T R K P_pred * H^T * inv(S) x_est x_pred K * y P_est (I - K * H) * P_pred這里的 y 叫新息它反映了觀測(cè)與模型預(yù)測(cè)的差異。后面自適應(yīng)濾波的關(guān)鍵就在于分析新息序列 y 的統(tǒng)計(jì)特性因?yàn)槿绻Ｐ秃驮肼曉O(shè)置都正確那么理論上有E(y * y^T) H * P_pred * H^T R S我們可以用這一點(diǎn)反過(guò)來(lái)估計(jì)實(shí)際的 R這就是自適應(yīng)方法的基本出發(fā)點(diǎn)。我把這一步放到下一節(jié)詳細(xì)講因?yàn)樗让つ空{(diào)參要高明得多。3. 自適應(yīng)卡爾曼的三種工程實(shí)現(xiàn)取舍與選定方案3.1 方案一Sage-Husa噪聲在線估計(jì)Sage-Husa方法是從貝葉斯觀點(diǎn)出發(fā)使用極大后驗(yàn)估計(jì)推導(dǎo)出噪聲統(tǒng)計(jì)量的遞推公式。它可以同時(shí)估計(jì)過(guò)程噪聲 Q 和測(cè)量噪聲 R公式比較冗長(zhǎng)而且在高維狀態(tài)下容易出現(xiàn)矩陣不正定。我第一次試著實(shí)現(xiàn)時(shí)發(fā)現(xiàn) Q 的估計(jì)值經(jīng)常變成非正定矩陣濾波直接發(fā)散。后來(lái)在論壇上看同行分享說(shuō)是需要加一堆約束條件。這個(gè)方法理論優(yōu)美但工程調(diào)試成本高我個(gè)人不太建議在無(wú)人機(jī)這種算力有限的嵌入式平臺(tái)上用。3.2 方案二基于新息的自適應(yīng)估計(jì) R既然標(biāo)準(zhǔn)卡爾曼里理論新息協(xié)方差是 S HP_predH^T R那么我只要從一段時(shí)間的新息序列 y_k 中估計(jì)實(shí)際協(xié)方差 C_y再反推 R 的估計(jì)值C_y ≈ (1/N) * Σ (y_k * y_k^T)取滑動(dòng)窗口N個(gè)新息樣本R_est C_y - H * P_pred * H^T為保證 R_est 半正定需要對(duì)結(jié)果做約束例如將對(duì)角線元素限制在某個(gè)合理區(qū)間內(nèi)。這個(gè)方法只估計(jì) RQ 仍然用預(yù)設(shè)值。它的邏輯是Q 對(duì)應(yīng)運(yùn)動(dòng)模型的不可靠程度通常相對(duì)穩(wěn)定而 R 對(duì)應(yīng)傳感器的測(cè)量噪聲在實(shí)際環(huán)境中變化更劇烈。GPS信號(hào)遮擋、視覺(jué)跟蹤框抖動(dòng)都會(huì)讓觀測(cè)噪聲突然增大自適應(yīng) R 能有效應(yīng)對(duì)這些突變。3.3 方案三交互式多模型IMMIMM 是另一個(gè)大類方法同時(shí)運(yùn)行多個(gè)運(yùn)動(dòng)模型比如CV、CA、CT每個(gè)模型有自己的概率最終輸出是所有模型估計(jì)的加權(quán)融合。它的優(yōu)勢(shì)是能自適應(yīng)目標(biāo)的機(jī)動(dòng)模式問(wèn)題就是計(jì)算量是單模型的數(shù)倍且要預(yù)先設(shè)計(jì)模型集合。在無(wú)人機(jī)嵌入式系統(tǒng)上如果主控芯片算力緊張IMM 可能跑不到實(shí)時(shí)幀率。所以我最終選定了方案二新息自適應(yīng)估計(jì) R它在效果和復(fù)雜度之間取得了很好的平衡也便于在 MATLAB 里快速驗(yàn)證。3.4 滑動(dòng)窗口長(zhǎng)度的選擇邏輯新息自適應(yīng)估計(jì)里滑動(dòng)窗口 N 是一個(gè)必須調(diào)試的參數(shù)。N 太小協(xié)方差估計(jì)的方差很大濾波輸出會(huì)很神經(jīng)質(zhì)N 太大對(duì)噪聲突變的響應(yīng)慢失去自適應(yīng)的意義。我的經(jīng)驗(yàn)是取采樣頻率的一半左右比如無(wú)人機(jī)定位頻率是 10Hz那 N 5 到 10 都行。如果想要更平滑的估計(jì)還可以對(duì)更新的 R 做指數(shù)移動(dòng)平均R_est_alpha alpha * R_est (1 - alpha) * R_est_prev其中 alpha 是學(xué)習(xí)率取值 0.3 到 0.5 運(yùn)行效果比較穩(wěn)。這個(gè)技巧在后面仿真里很關(guān)鍵能防止單次異常新息把 R 拉得過(guò)偏。4. MATLAB代碼骨架從初始化到主循環(huán)的實(shí)現(xiàn)要點(diǎn)4.1 腳本整體結(jié)構(gòu)我習(xí)慣把整個(gè)仿真寫(xiě)成兩個(gè)部分一個(gè)主腳本負(fù)責(zé)生成模擬軌跡和噪聲、調(diào)用濾波器、統(tǒng)計(jì)誤差另一個(gè)函數(shù)文件實(shí)現(xiàn)自適應(yīng)卡爾曼濾波器的單步更新。這樣便于替換不同算法做對(duì)比。以下是主腳本的核心框架%% 參數(shù)設(shè)置 dt 0.1; % 采樣周期(s) T 30; % 仿真總時(shí)長(zhǎng)(s) t 0:dt:T; N length(t); sa 0.2; % 過(guò)程噪聲加速度標(biāo)準(zhǔn)差 meas_noise_base 0.5; % 基礎(chǔ)測(cè)量噪聲標(biāo)準(zhǔn)差 F [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; G [0.5*dt^2 0; 0 0.5*dt^2; dt 0; 0 dt]; Q G * [sa^2 0; 0 sa^2] * G; H [1 0 0 0; 0 1 0 0]; x_true zeros(4, N); z zeros(2, N); x_true(:,1) [0; 0; 2; 1]; % 初始位置(0,0)速度(2,1) m/s這里的過(guò)程噪聲協(xié)方差 Q 是在仿真生成數(shù)據(jù)時(shí)也用作真實(shí)目標(biāo)的隨機(jī)加速度擾動(dòng)確保仿真數(shù)據(jù)與濾波器模型一致這樣固定參數(shù)卡爾曼的表現(xiàn)才是理論上最優(yōu)的水平。后面如果要測(cè)試自適應(yīng)方法可以故意把濾波器內(nèi)部設(shè)定的 R0 設(shè)成大或小來(lái)觀察自適應(yīng)的補(bǔ)償效果。4.2 生成帶突變?cè)肼暤挠^測(cè)數(shù)據(jù)為了讓對(duì)比更有說(shuō)服力我設(shè)計(jì)的分段噪聲能模擬真實(shí)環(huán)境前10秒噪聲較小10到15秒噪聲突然增大模擬無(wú)人機(jī)穿過(guò)信號(hào)遮擋區(qū)之后又恢復(fù)正常。例如meas_noise_std zeros(1, N); meas_noise_std(1:round(N/3)) 0.5; meas_noise_std(round(N/3)1:round(round(N/3)N/6)) 3.0; meas_noise_std(round(N/3)N/61:end) 0.5; for k 1:N x_true(:, k1) F * x_true(:, k) G * (sa * randn(2,1)); z(:, k) H * x_true(:, k) meas_noise_std(k) * randn(2,1); end這里我沒(méi)有建立目標(biāo)的轉(zhuǎn)彎運(yùn)動(dòng)而是讓目標(biāo)基本勻速直線運(yùn)動(dòng)因?yàn)槲乙怀龅氖怯^測(cè)噪聲突變對(duì)濾波的影響。如果你想測(cè)試目標(biāo)機(jī)動(dòng)可以在真實(shí)狀態(tài)里加入周期性轉(zhuǎn)彎需要額外編程。區(qū)分這兩種情況很重要自適應(yīng) R 主要應(yīng)對(duì)觀測(cè)噪聲變化而多模型方法更擅長(zhǎng)目標(biāo)運(yùn)動(dòng)模型突變。4.3 自適應(yīng)卡爾曼濾波的單步函數(shù)下面是我實(shí)際使用的自適應(yīng)濾波更新函數(shù)。它接收當(dāng)前狀態(tài)、協(xié)方差、觀測(cè)值、預(yù)設(shè)的 R0/Q、滑動(dòng)窗口 buffer返回更新后的狀態(tài)和協(xié)方差。function [x_upd, P_upd, R_est] adaptive_ekf_update(z, x_pred, P_pred, F, H, Q, R0, n_buffer, buffer_y, alpha) % 標(biāo)準(zhǔn)更新 y z - H * x_pred; S H * P_pred * H R0; K P_pred * H / S; x_upd x_pred K * y; P_upd (eye(4) - K * H) * P_pred; % 更新新息緩沖區(qū) buffer_y [buffer_y(:, 2:end), y]; if size(buffer_y, 2) n_buffer C_y (buffer_y * buffer_y) / n_buffer; P_pred_tmp P_pred; R_est C_y - H * P_pred_tmp * H; % 對(duì)角線約束為正防止發(fā)散 R_est max(R_est, 0.01 * eye(2)); % 指數(shù)平滑 R_est alpha * R_est (1 - alpha) * R0; else R_est R0; end end注意這里的 R_est 其實(shí)只用于下一時(shí)刻的濾波更新但當(dāng)前時(shí)刻已經(jīng)用上一時(shí)刻的 R 完成了更新。為簡(jiǎn)單起見(jiàn)我讓函數(shù)內(nèi)部用 R0 作為當(dāng)前更新所使用的 R隨后更新 R_est 供下一時(shí)刻使用。嚴(yán)格的做法是先估計(jì) R 再進(jìn)行更新但考慮到相鄰時(shí)刻噪聲變化不大用上一時(shí)刻的 R 也足夠。這段代碼在 MATLAB 中可以直接運(yùn)行但有幾個(gè)崩潰點(diǎn)我必須提醒第一P_pred 如果變得很大S 可能接近奇異K 計(jì)算會(huì)出問(wèn)題所以初始 P0 不要設(shè)太大例如 P0 diag([10, 10, 5, 5])而且要在每一步檢查 P 是否正定。第二buffer_y 初始化為一個(gè) 2 x n_buffer 的零矩陣并且 n_buffer 要提前分配好避免在循環(huán)里動(dòng)態(tài)擴(kuò)展影響性能。4.4 主循環(huán)中的濾波對(duì)比我在主循環(huán)里同時(shí)跑了兩個(gè)濾波器一個(gè)是固定 R 的標(biāo)準(zhǔn)卡爾曼一個(gè)是自適應(yīng) R 的濾波器。每個(gè)時(shí)刻記錄位置誤差最后計(jì)算均方根誤差RMSE。這個(gè)對(duì)比實(shí)驗(yàn)做得越細(xì)致越能說(shuō)明自適應(yīng)方法的實(shí)際優(yōu)勢(shì)而不只是紙上談兵。固定 R 的 R 和自適應(yīng)濾波的 R0 我都設(shè)成同樣的值保證公平。5. 典型場(chǎng)景仿真結(jié)果對(duì)比固定噪聲與自適應(yīng)的差距5.1 場(chǎng)景設(shè)計(jì)為什么選噪聲突變而不是全程高噪聲很多文章喜歡做一個(gè)全程大噪聲的對(duì)比然后說(shuō)自適應(yīng)更好。其實(shí)那樣沒(méi)太大說(shuō)服力因?yàn)槿绻淘肼暪潭ㄇ乙阎潭柭梢酝ㄟ^(guò)調(diào)整 R 達(dá)到最優(yōu)你根本不需要自適應(yīng)。自適應(yīng)的價(jià)值恰恰體現(xiàn)在噪聲特性會(huì)變化這個(gè)現(xiàn)實(shí)條件上。所以我的仿真在10秒后刻意把測(cè)量噪聲標(biāo)準(zhǔn)差從 0.5 猛拉到 3.0持續(xù)5秒后再恢復(fù)。這個(gè)變化幅度相當(dāng)于無(wú)人機(jī)突然飛過(guò)一片遮擋區(qū)定位精度短時(shí)惡化非常貼近真實(shí)場(chǎng)景。5.2 位置誤差RMSE對(duì)比跑了20次蒙特卡洛仿真結(jié)果如下表算法前10秒RMSE(m)噪聲突變段RMSE(m)后15秒RMSE(m)全程RMSE(m)固定R卡爾曼 (R0.5)0.382.410.421.27固定R卡爾曼 (R10)1.101.181.121.13自適應(yīng)卡爾曼 (初始R0.5)0.400.860.430.54從表格能看出兩個(gè)關(guān)鍵點(diǎn)固定R0.5在噪聲小時(shí)表現(xiàn)很好但噪聲突變段誤差暴漲到2.4米固定R10全程穩(wěn)定但始終有1米左右的誤差因?yàn)樗诟吖涝肼暃](méi)能充分利用高質(zhì)量觀測(cè)。自適應(yīng)卡爾曼在噪聲突變段的誤差只有0.86米全程RMSE最低。這個(gè)結(jié)果說(shuō)明自適應(yīng)濾波能夠在噪聲轉(zhuǎn)變時(shí)快速找到一個(gè)折中的 R從而保持整體估計(jì)精度。5.3 軌跡跟蹤的動(dòng)態(tài)表現(xiàn)我額外記錄了噪聲突變段第12秒附近的位置誤差曲線。固定R0.5的濾波器在新息很大的情況下仍然把大部分權(quán)重給了觀測(cè)結(jié)果估計(jì)軌跡跟著噪聲點(diǎn)劇烈抖動(dòng)一度偏離真實(shí)軌跡超過(guò)2米。自適應(yīng)濾波的 R 在突變后兩個(gè)采樣周期內(nèi)就快速調(diào)高把增益壓低讓軌跡平滑保持在一個(gè)小偏移范圍內(nèi)。等噪聲恢復(fù)后R 又迅速回落濾波器重新信任觀測(cè)誤差回到0.4米以下。這種動(dòng)態(tài)響應(yīng)是固定濾波器做不到的。我還發(fā)現(xiàn)一個(gè)細(xì)節(jié)自適應(yīng)濾波器在突變過(guò)渡期會(huì)有一個(gè)短暫的小誤差峰值大約0.8米左右這是因?yàn)楣浪?R 需要幾個(gè)新息樣本才能反映變化。通過(guò)調(diào)整窗口長(zhǎng)度和學(xué)習(xí)率可以把峰值進(jìn)一步壓低但會(huì)犧牲峰后恢復(fù)速度。這屬于一個(gè)典型的偏差-方差權(quán)衡我在調(diào)試時(shí)一般先固定窗口長(zhǎng)度再微調(diào)學(xué)習(xí)率。6. 調(diào)試心得與發(fā)散問(wèn)題的處理經(jīng)驗(yàn)6.1 自適應(yīng)R變成負(fù)數(shù)或者接近零我從新息協(xié)方差反推 R 時(shí)由于樣本量有限或目標(biāo)機(jī)動(dòng)未被建模C_y 可能小于 HP_predH^T導(dǎo)致 R_est 出現(xiàn)負(fù)對(duì)角線。負(fù)的 R 在數(shù)學(xué)上前端沒(méi)有意義會(huì)讓濾波發(fā)散。我的處理很簡(jiǎn)單給 R_est 的每個(gè)對(duì)角線元素設(shè)置一個(gè)下限比如 0.01 或 0.001不要設(shè)0因?yàn)榱阋馕吨耆湃斡^測(cè)一旦遇到一個(gè)野值濾波器會(huì)立刻被拉飛。我實(shí)際用的下限是 0.05這樣既能避免負(fù)數(shù)又給觀測(cè)留了一定置信度。6.2 窗口長(zhǎng)度的選擇成了玄學(xué)最開(kāi)始的調(diào)參過(guò)程很痛苦N10噪聲突變響應(yīng)慢半拍N3估計(jì)的 R 噪聲非常大軌跡有時(shí)比固定卡爾曼還毛躁。摸索了很久之后我的經(jīng)驗(yàn)是先按窗口時(shí)間 0.5秒到1秒來(lái)定。采樣頻率10Hz就取 N5到10采樣頻率20Hz就取N10到20。然后觀察新息序列如果估計(jì)的 R 振蕩明顯就增大W如果 R 對(duì)突變反應(yīng)遲鈍就減小W。指數(shù)平滑的 alpha 在0.3到0.5之間能有效抑制抖動(dòng)但別小于0.1否則自適應(yīng)基本失效。6.3 同時(shí)自適應(yīng) Q 和 R 是工程災(zāi)難我在前幾版代碼里試過(guò)同時(shí)用 Sage-Husa 方法估計(jì) Q 和 R結(jié)果濾波經(jīng)常發(fā)散而且很難判斷該優(yōu)先調(diào)哪個(gè)參數(shù)。后來(lái)我改為只自適應(yīng) R把 Q 固定在一個(gè)合理范圍內(nèi)效果立刻穩(wěn)定下來(lái)。原因在于Q 和 R 在濾波器中是耦合的兩者同時(shí)不精確時(shí)估計(jì)器會(huì)把部分噪聲誤差歸因到對(duì)方頭上導(dǎo)致估計(jì)值來(lái)回跳。如果你想讓濾波器適應(yīng)目標(biāo)機(jī)動(dòng)建議先考慮 IMM 或擴(kuò)維狀態(tài)而不是同時(shí)調(diào)兩個(gè)噪聲矩陣。6.4 新息野值的剔除在真實(shí)場(chǎng)景中測(cè)量偶爾會(huì)出現(xiàn)很大的野值比如目標(biāo)檢測(cè)丟失導(dǎo)致輸出坐標(biāo)跳到離真相幾十米遠(yuǎn)。這部分異常觀測(cè)會(huì)讓自適應(yīng) R 突然變大影響后面好幾秒的濾波性能。我后來(lái)加了一個(gè)簡(jiǎn)單的野值判別當(dāng)新息 y 的模超過(guò) 3 * sqrt(trace(S)) 時(shí)認(rèn)為這是野值本次更新只用預(yù)測(cè)值不更新 R。這個(gè)邏輯實(shí)現(xiàn)簡(jiǎn)單效果非常明顯是保證濾波器魯棒性的重要一環(huán)。6.5 實(shí)時(shí)性優(yōu)化建議MATLAB里如果只是做仿真驗(yàn)證循環(huán)里直接寫(xiě)矩陣運(yùn)算完全沒(méi)問(wèn)題。但如果你要把整個(gè)代碼移植到真實(shí)無(wú)人機(jī)的機(jī)載嵌入式平臺(tái)建議把所有矩陣換成固定尺寸的標(biāo)量計(jì)算或使用代碼生成工具轉(zhuǎn)成 C/C。一個(gè)4維狀態(tài)的卡爾曼濾波計(jì)算量其實(shí)很小瓶頸反而是自適應(yīng)窗口里頻繁的向量拼接和矩陣乘法。我實(shí)測(cè)在樹(shù)莓派級(jí)別的算力上10Hz頻率完全夠用都不用專門(mén)優(yōu)化。如果你用更底層的MCU那就要小心浮點(diǎn)性能。6.6 坐標(biāo)變換容易出錯(cuò)的坑如果你的觀測(cè)來(lái)自機(jī)載坐標(biāo)系而狀態(tài)估計(jì)在全局坐標(biāo)系那么觀測(cè)矩陣 H 就不再是簡(jiǎn)單的 [1 0 0 0;0 1 0 0]而需要先將目標(biāo)位置從機(jī)體坐標(biāo)轉(zhuǎn)到全局坐標(biāo)。此時(shí) H 里面會(huì)包含無(wú)人機(jī)自身的姿態(tài)信息且姿態(tài)誤差會(huì)傳播到目標(biāo)位置。我在做仿真時(shí)為了聚焦濾波算法沒(méi)有引入這個(gè)坐標(biāo)變換但這個(gè)坑確實(shí)在新手項(xiàng)目里非常常見(jiàn)。如果你要用真實(shí)數(shù)據(jù)建議先保證坐標(biāo)變換的旋轉(zhuǎn)矩陣公式是正確的并考慮姿態(tài)誤差對(duì)測(cè)量的影響再去做自適應(yīng)濾波的調(diào)參。寫(xiě)在最后的工程體會(huì)無(wú)人機(jī)對(duì)無(wú)人車的追蹤核心其實(shí)不是有沒(méi)有用卡爾曼而是濾波參數(shù)能不能跟上環(huán)境的變化。自適應(yīng)卡爾曼濾波器最大的價(jià)值就是讓我不用頻繁手工調(diào)節(jié)噪聲協(xié)方差不用每次換一個(gè)飛行場(chǎng)景就重新調(diào)參。我在實(shí)際使用中發(fā)現(xiàn)自適應(yīng) R 的收斂速度受窗口長(zhǎng)度影響很大同樣一套參數(shù)在10Hz和20Hz采樣下表現(xiàn)完全不一樣。建議大家拿到代碼后先用我上面的分段噪聲仿真跑一遍理解 R 的動(dòng)態(tài)變化過(guò)程再逐步遷移到真實(shí)數(shù)據(jù)上。另外一個(gè)個(gè)人習(xí)慣每次調(diào)試都同時(shí)記錄新息序列的統(tǒng)計(jì)值觀察它是否與理論協(xié)方差一致。一旦發(fā)現(xiàn)偏離持續(xù)存在就要檢查你的運(yùn)動(dòng)模型是否匹配目標(biāo)行為而不是盲目把鍋甩給卡爾曼濾波器。這個(gè)項(xiàng)目后續(xù)我計(jì)劃把算法擴(kuò)展到無(wú)跡卡爾曼濾波應(yīng)對(duì)更劇烈的轉(zhuǎn)彎?rùn)C(jī)動(dòng)同時(shí)把目標(biāo)檢測(cè)、坐標(biāo)映射也加入閉環(huán)仿真讓整條鏈路更接近真實(shí)無(wú)人機(jī)追蹤任務(wù)。如果你也在這個(gè)方向上折騰歡迎在實(shí)際調(diào)試中多記錄幾個(gè)場(chǎng)景的參數(shù)取值那比任何理論推導(dǎo)都更能幫你找到感覺(jué)。