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