做雷達目標(biāo)跟蹤的人基本都繞不開卡爾曼濾波器。你手里有一堆雷達量測點跡帶著噪聲、漏檢、虛警要把它變成一條平滑、可用、能預(yù)測下一幀位置的目標(biāo)軌跡最經(jīng)典的做法就是卡爾曼濾波。我這次在Matlab里把七種常見變體——基本離散Kalman、固定增益Kalman、平方根Kalman、遺忘因子Kalman、擴大P Kalman、自適應(yīng)Kalman、有限K減小Kalman——逐個實現(xiàn)了一遍用同一段雷達軌跡數(shù)據(jù)做了對比。整個過程踩了不少坑這篇就把代碼思路、公式取舍和調(diào)參經(jīng)驗一次說清楚。先說結(jié)論這七種變體不是教科書湊篇幅用的每一種都對應(yīng)一類真實的工程困境?；倦x散Kalman是地基平方根Kalman治的是數(shù)值病態(tài)遺忘因子和自適應(yīng)對付的是目標(biāo)機動與噪聲未知擴大P和有限K減小是兩招應(yīng)急補救固定增益則是算力受限時的妥協(xié)方案。我建議你把這篇文章當(dāng)成一份“選型筆記”來讀而不是單純抄代碼。1. 整體設(shè)計思路七種變體不是堆料是七種工程困境的對癥藥1.1 核心需求拆解雷達軌跡濾波里Kalman到底在干什么雷達跟蹤的基本場景是這樣的雷達周期性地給出目標(biāo)點跡通常包含距離、方位角、俯仰角或者已經(jīng)轉(zhuǎn)換到直角坐標(biāo)系下的X、Y坐標(biāo)。這些量測天生帶有噪聲而且噪聲統(tǒng)計特性不完全已知?？柭鼮V波要做的事就是利用目標(biāo)的運動模型比如勻速模型、勻加速模型和量測模型把這兩路信息按協(xié)方差加權(quán)融合輸出一個比原始量測更接近真實位置的狀態(tài)估計。在標(biāo)準的離散線性系統(tǒng)里狀態(tài)方程和量測方程寫作x(k1) F * x(k) G * w(k) z(k) H * x(k) v(k)其中F是狀態(tài)轉(zhuǎn)移矩陣H是量測矩陣w是過程噪聲v是量測噪聲?？柭鼮V波的核心是每一步做兩個動作先用狀態(tài)方程做“預(yù)測”再用帶噪聲的量測做“修正”。預(yù)測結(jié)果的可靠程度由誤差協(xié)方差矩陣P描述修正力度則體現(xiàn)為卡爾曼增益K。聽起來很簡單但工程上一旦跑起來問題就全出來了P矩陣可能因為數(shù)值舍入失去對稱正定性目標(biāo)可能突然轉(zhuǎn)彎導(dǎo)致模型失配量測噪聲方差估計不準導(dǎo)致濾波發(fā)散算力不夠用沒法每幀求逆矩陣。標(biāo)題里那七個名字本質(zhì)上就是針對這些痛點長出來的“補丁”。1.2 七種變體對應(yīng)的問題定位與選型參考我把七種變體按“解決什么問題”重新排了一張表讓你一眼就能找到自己該用哪一種變體解決的核心問題典型適用場景基本離散Kalman線性高斯系統(tǒng)的最優(yōu)遞推估計基線目標(biāo)運動規(guī)律明確、噪聲統(tǒng)計已知、算力充足固定增益Kalman在線計算量太大P矩陣和K矩陣不必每幀更新嵌入式實時系統(tǒng)、穩(wěn)態(tài)長時跟蹤平方根KalmanP矩陣因舍入誤差失去正定性濾波崩潰長時間運行、高維狀態(tài)、計算機字長受限遺忘因子Kalman模型失配或環(huán)境突變舊數(shù)據(jù)權(quán)重過高機動目標(biāo)跟蹤、時變參數(shù)估計擴大P Kalman濾波已出現(xiàn)發(fā)散征兆需要快速增強量測權(quán)重突發(fā)機動、目標(biāo)丟失后重新捕獲自適應(yīng)KalmanQ和R不準確或時變需要在線估計噪聲統(tǒng)計雷達噪聲隨環(huán)境變化、缺乏準確標(biāo)定數(shù)據(jù)有限K減小KalmanK收斂到過小導(dǎo)致濾波器對機動“反應(yīng)遲鈍”長時跟蹤中需要保留突發(fā)機動響應(yīng)能力如果你的項目是跑離線數(shù)據(jù)我建議先把基本離散Kalman調(diào)通再疊加平方根和自適應(yīng)如果是上實時平臺固定增益和有限K減小是更現(xiàn)實的選擇。下面我從基本離散Kalman講起。2. 基本離散Kalman先把地基打牢后面的變體都是在改這一行2.1 基本離散Kalman的遞推循環(huán)預(yù)測、增益、修正基本離散Kalman一共五條公式分成預(yù)測和更新兩組。預(yù)測階段x_pred F * x_prev P_pred F * P_prev * F Q更新階段K P_pred * H * inv(H * P_pred * H R) x_post x_pred K * (z - H * x_pred) P_post (I - K * H) * P_pred注意最后一條P_post (I - K*H) * P_pred叫“減法形式”計算量小但做減法會破壞對稱正定性長時間運行容易出現(xiàn)P矩陣非正定。更穩(wěn)的寫法是Joseph形式P_post (I - K*H) * P_pred * (I - K*H) K * R * KJoseph形式多算一次矩陣乘但對稱性和正定性保持得更好。在Matlab里這一行代碼的差別就是濾波結(jié)果“偶爾崩”和“一直穩(wěn)”之間的差別。2.2 可運行的Matlab核心代碼從軌跡生成到濾波閉環(huán)我建議所有變體都基于同一個仿真主框架這樣對比才公平。先建一個最簡單的勻速目標(biāo)軌跡目標(biāo)在XY平面內(nèi)運動雷達每0.1秒給一組帶噪聲的XY坐標(biāo)量測。% 基本仿真參數(shù) dt 0.1; T 50; % 總時長5秒 t 0:dt:T; N length(t); % 真實軌跡x方向勻速y方向帶一點速度變化 true_x zeros(1, N); true_y zeros(1, N); true_vx 150 * ones(1, N); % 150 m/s true_vy 100 * ones(1, N); true_x(1) 0; true_y(1) 0; for k 2:N true_x(k) true_x(k-1) true_vx(k-1) * dt; true_y(k) true_y(k-1) true_vy(k-1) * dt; end % 雷達量測真實位置 高斯噪聲 R_true diag([20, 20]); % 量測噪聲協(xié)方差 meas_x true_x sqrt(R_true(1,1)) * randn(1, N); meas_y true_y sqrt(R_true(2,2)) * randn(1, N); % 狀態(tài)向量 [x; y; vx; vy] F [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; H [1 0 0 0; 0 1 0 0]; % 過程噪聲協(xié)方差離散白噪聲加速度模型 q 10; % 過程噪聲強度需要調(diào) Q q * [dt^3/3 0 dt^2/2 0; 0 dt^3/3 0 dt^2/2; dt^2/2 0 dt 0; 0 dt^2/2 0 dt]; % 初始狀態(tài) x_est [meas_x(1); meas_y(1); 0; 0]; P_est diag([20, 20, 1000, 1000]); % 存儲濾波結(jié)果 est_x zeros(1, N); est_y zeros(1, N); for k 1:N % 預(yù)測 x_pred F * x_est; P_pred F * P_est * F Q; % 更新 S H * P_pred * H R_true; K P_pred * H / S; % 用 / 而不是 inv(S)*H innov [meas_x(k) - x_pred(1); meas_y(k) - x_pred(2)]; x_est x_pred K * innov; I eye(4); P_est (I - K*H) * P_pred * (I - K*H) K * R_true * K; est_x(k) x_est(1); est_y(k) x_est(2); end這段代碼跑通之后你可以用plot(true_x, true_y, meas_x, meas_y, est_x, est_y)畫三條線驗證效果。2.3 調(diào)參新手最容易踩的坑Q和R的絕對值意義很多新手調(diào)不好卡爾曼問題出在把Q和R當(dāng)成了“兩個可以隨意瞎擰的旋鈕”。實際上它們有明確物理含義R是量測噪聲方差你可以從量測數(shù)據(jù)里直接統(tǒng)計出來Q是過程噪聲協(xié)方差描述你對運動模型的信任程度。我見過一個典型錯誤目標(biāo)勻速直線運動量測噪聲其實很小卻把R設(shè)成eye(2)結(jié)果濾波器對量測噪聲“太寬容”軌跡毛刺一大堆。反過來如果R設(shè)得過小濾波器會瘋狂相信量測目標(biāo)位置會在真值附近劇烈抖動。我的調(diào)參順序是這樣的先用一段靜止目標(biāo)數(shù)據(jù)統(tǒng)計量測噪聲方差得到R的基準值然后保持R不變從很小的Q開始往上加直到濾波軌跡和真值之間的RMSE不再顯著下降就停下來。Q調(diào)大意味著你更相信量測調(diào)小意味著更相信模型。這個比值關(guān)系比單個絕對數(shù)值重要得多。3. 平方根Kalman和遺忘因子Kalman數(shù)值穩(wěn)定性和機動目標(biāo)的兩劑猛藥3.1 平方根Kalman為什么P矩陣會“病”了以及怎么用Cholesky因子救基本離散Kalman跑短時間沒問題但長時間運行尤其是狀態(tài)維數(shù)高、量測噪聲很小的時候P矩陣會因為計算舍入誤差逐漸失去對稱正定性。一旦P不滿足正定卡爾曼增益K可能算出接近零的離譜值濾波直接發(fā)散。這是數(shù)值線性代數(shù)的經(jīng)典問題不是算法邏輯錯誤。平方根Kalman的思路是把誤差協(xié)方差P做Cholesky分解P S * S遞推過程中始終維護S而不是P。因為S是三角矩陣S*S在數(shù)學(xué)上自動保證半正定即使數(shù)值上有微小誤差也不容易出現(xiàn)“負方差”這種荒謬結(jié)果。嚴格的平方根Kalman實現(xiàn)一般用QR分解或Cholesky更新來遞推S但Matlab里你可以先用一種直觀的簡化版本每幀先用常規(guī)方式計算P然后做一次chol(P, upper)把三角因子拿出來。這樣做性能不是最優(yōu)但代碼可讀性強很多也足夠解決大部分數(shù)值病態(tài)問題。% 平方根Kalman簡化版核心 S chol(P0, upper); for k 1:N % 預(yù)測 P_pred F * (S * S) * F Q; S_pred chol(P_pred, upper); % 重新分解一次 x_pred F * x_est; % 更新 S_innov H * P_pred * H R_true; K P_pred * H / S_innov; innov [meas_x(k) - x_pred(1); meas_y(k) - x_pred(2)]; x_est x_pred K * innov; P_post (eye(4) - K*H) * P_pred * (eye(4) - K*H) K * R_true * K; S chol(P_post, upper); end這里每幀都重新做一次Cholesky分解只取了“用三角因子保存P”的思想。真正上飛行器或嵌入式平臺時建議改用qr矩陣分解實現(xiàn)一步遞推計算效率高一個量級。如果你只是做雷達軌跡離線分析這個簡化版足夠用了。3.2 遺忘因子Kalman讓舊量測主動“過期”模型失配不硬扛基本離散Kalman對過去所有量測的權(quán)重是一視同仁的但雷達目標(biāo)經(jīng)常不按常理出牌前一秒勻速直線下一秒突然轉(zhuǎn)彎。這時濾波器還守著幾十幀前的老模型就會出現(xiàn)明顯的跟蹤滯后。遺忘因子Kalman的核心思想是“讓舊數(shù)據(jù)逐漸過期”常用做法是在預(yù)測協(xié)方差上乘一個略大于1的加權(quán)系數(shù)lambdaP_pred lambda * F * P_prev * F Qlambda取1.01到1.05之間的值時每步預(yù)測的不確定性被輕微放大卡爾曼增益K隨之變大新量測在估計中的權(quán)重提高。這樣一來目標(biāo)機動時濾波器能更快“忘掉”過時的運動模型。lambda 1.02; % 遺忘因子越大對新量測越敏感 for k 1:N x_pred F * x_est; P_pred lambda * F * P_est * F Q; % 關(guān)鍵改動就這一行 S H * P_pred * H R_true; K P_pred * H / S; innov [meas_x(k) - x_pred(1); meas_y(k) - x_pred(2)]; x_est x_pred K * innov; P_est (eye(4) - K*H) * P_pred; endlambda不是越大越好。我實測下來lambda超過1.1之后濾波對噪聲的敏感度急劇上升軌跡會出現(xiàn)明顯的“鋸齒感”。最佳值取決于目標(biāo)機動頻率一般從1.02開始試。3.3 兩種變體的Matlab實現(xiàn)要點平方根和遺忘因子可以疊加使用先用平方根方式保證P矩陣數(shù)值穩(wěn)定再在P_pred上乘遺忘因子。但要注意遺忘因子乘大P_pred后P矩陣的“膨脹”會抵消一部分平方根帶來的穩(wěn)定性優(yōu)勢所以兩者的參數(shù)不能都拉滿。我的經(jīng)驗是平方根負責(zé)保底遺忘因子只加在機動段的前幾幀等重新捕獲目標(biāo)后就把lambda恢復(fù)成1.00。另一個容易踩的坑是Matlab里chol默認返回下三角矩陣和論文里常用的S定義不一定一致。你只要保證前后一致就行別一會兒用上三角一會兒用下三角否則S*S的順序會亂代碼直接報錯。4. 自適應(yīng)Kalman與擴大P應(yīng)對噪聲未知和突發(fā)機動4.1 自適應(yīng)Kalman用新息序列在線估計R前面所有的變體都假設(shè)量測噪聲協(xié)方差R是已知的固定值。但雷達環(huán)境會變雷達從跟蹤遠距離小目標(biāo)切換到近距離大目標(biāo)量測噪聲特性可能完全不同。這時候如果還抱著一個固定的R濾波性能會明顯下降。自適應(yīng)Kalman里最實用的思路是“新息協(xié)方差匹配法”。新息innov z - H*x_pred理論上應(yīng)該服從均值為零、協(xié)方差為S H*P_pred*H R的高斯分布。如果R估計不準新息的實際協(xié)方差和理論協(xié)方差就不一致于是可以反推R的估計值R_hat (1/N) * sum(innov * innov) - H * P_pred * H實際工程中我一般維護一個滑動窗口保存最近20到50幀的新息用窗口內(nèi)的樣本協(xié)方差去更新R_hat并且強制加一個下限防止估計出負方差。window 30; innov_buffer zeros(2, window); R_meas R_true; % 初始值 for k 1:N x_pred F * x_est; P_pred F * P_est * F Q; S H * P_pred * H R_meas; K P_pred * H / S; innov [meas_x(k) - x_pred(1); meas_y(k) - x_pred(2)]; % 緩存新息并滑動更新R估計 idx mod(k-1, window) 1; innov_buffer(:, idx) innov; if k window innov_cov cov(innov_buffer); R_est innov_cov - H * P_pred * H; R_est max(R_est, diag([5, 5])); % 下限保護 R_meas 0.9 * R_meas 0.1 * R_est; % 平滑防止跳變 end x_est x_pred K * innov; P_est (eye(4) - K*H) * P_pred; end一定要加平滑系數(shù)我試過直接讓R_meas R_est結(jié)果R在個別幀劇烈跳變?yōu)V波反而發(fā)散。用0.9/0.1這種遞推加權(quán)可以讓R緩慢跟隨環(huán)境變化。4.2 擴大P發(fā)散預(yù)警后的一腳地板油擴大P Kalman是應(yīng)對“濾波已經(jīng)不行了”的應(yīng)急手段。判斷濾波發(fā)散的標(biāo)準有很多最常用的是卡方檢驗計算歸一化新息平方NIS當(dāng)它超過某個門限時認為模型和量測嚴重不匹配。NIS innov / (H * P_pred * H R) * innovNIS在4維量測下大致服從卡方分布常用的檢測門限可以取9到16之間。一旦觸發(fā)就把P_pred放大一個倍數(shù)比如乘以10下一幀的卡爾曼增益K會同步變大濾波器能快速拉回目標(biāo)附近。for k 1:N x_pred F * x_est; P_pred F * P_est * F Q; innov [meas_x(k) - x_pred(1); meas_y(k) - x_pred(2)]; S H * P_pred * H R_true; NIS innov / S * innov; if NIS 16 P_pred 10 * P_pred; % 擴大P下一幀增強量測權(quán)重 end K P_pred * H / S; x_est x_pred K * innov; P_est (eye(4) - K*H) * P_pred; end擴大P不能連續(xù)觸發(fā)。如果連續(xù)好幾幀都NIS超限說明不是偶發(fā)機動而是運動模型整體失效這時候應(yīng)該切換模型或者重新初始化濾波器。連續(xù)觸發(fā)還硬靠擴P硬拉軌跡會抖得很厲害。4.3 兩類方法的聯(lián)動使用自適應(yīng)和擴大P在雷達跟蹤里經(jīng)常一起用。自適應(yīng)負責(zé)慢速調(diào)節(jié)R解決噪聲統(tǒng)計漂移擴大P負責(zé)快速響應(yīng)解決突發(fā)機動。但聯(lián)動時優(yōu)先級很重要自適應(yīng)更新R一定用擴P之前的舊新息否則會把機動引起的“大新息”誤判成“噪聲變大”導(dǎo)致R被撐得巨大。我自己的做法是先用NIS門限判斷是否擴P只有在NIS正常的情況下才把當(dāng)前新息放進滑動窗口去更新R。這套邏輯加進去之后代碼量不大但濾波的魯棒性明顯提升。5. 固定增益Kalman和有限K減小算力受限場景下的兩種“妥協(xié)方案”5.1 固定增益Kalman用離線收斂換在線算力在真實雷達系統(tǒng)中每一幀都要在極短時間內(nèi)完成濾波。基本離散Kalman每幀要算P_pred、H*P_pred*H R的逆、K、P_post矩陣維度不高時還好狀態(tài)維度一上去在線計算壓力就上來了。固定增益Kalman的想法是如果系統(tǒng)是線性時不變的P矩陣和K矩陣會隨著迭代逐漸收斂到一個穩(wěn)態(tài)值。既然穩(wěn)態(tài)值不變我何必每幀都算一遍直接離線迭代幾百幀把穩(wěn)態(tài)K取出來在線階段只做兩步x_pred F * x_prev x_post x_pred K_fixed * (z - H * x_pred)在線不再需要求逆也不再需要更新P計算量小了一個量級。% 離線階段迭代求穩(wěn)態(tài)K P_temp diag([20, 20, 1000, 1000]); for i 1:500 P_pred F * P_temp * F Q; K_temp P_pred * H / (H * P_pred * H R_true); P_temp (eye(4) - K_temp*H) * P_pred; end K_fixed K_temp; % 在線階段固定增益 x_est [meas_x(1); meas_y(1); 0; 0]; for k 2:N x_pred F * x_est; innov [meas_x(k) - x_pred(1); meas_y(k) - x_pred(2)]; x_est x_pred K_fixed * innov; end如果裝了Control System Toolbox可以用[~, K_fixed, ~] idare(F, H, Q, R_true)一步算出穩(wěn)態(tài)增益省掉迭代循環(huán)。沒用工具箱的話上面這個500次迭代也很快Matlab里基本瞬間完成。5.2 有限K減小防止K無限收斂保住機動響應(yīng)固定增益Kalman暴露的問題很明顯如果Q很小、R很大濾波器對量測的長期依賴度會越來越低K矩陣會收斂到一個很小的值。這時候目標(biāo)一旦機動新息再大增益也拉不動狀態(tài)濾波器的“反應(yīng)”會變得異常遲鈍。有限K減小Kalman的思路是給K矩陣設(shè)一個下限不允許它無限縮小。但直接對K矩陣逐元素設(shè)下限并不合適因為K的每個元素對應(yīng)不同狀態(tài)分量量綱都不一樣。更穩(wěn)的做法是對P矩陣施加約束當(dāng)P矩陣的特征值小于某個下限時把特征值強制抬升到一個保底值下一幀計算出來的K自然也不會太小。% 有限K減小限制P特征值下限 P_eig_min 50; [V, D] eig(P_est); D(D P_eig_min) P_eig_min; P_est V * D * V;注意eig分解本身比較耗時只是為了講解清楚才這么寫。實際工程里更高效的做法是判斷trace(P)或trace(K)是否低于門限再決定是否做特征值修正沒必要每幀都分解一次。5.3 何時用固定增益何時用有限K這兩個方案其實是一對互補。固定增益面向“平穩(wěn)長時跟蹤”省算力有限K減小面向“目標(biāo)隨時可能機動”保響應(yīng)。如果平臺算力確實緊張我推薦的做法是離線算好固定增益在線只保留一個K_min檢查每N幀檢查一次K的跡太低就臨時用有限K的邏輯抬一下P。這套組合我實測算力大概能比完整自適應(yīng)Kalman省一半左右代價是機動段的誤差會稍大。對于車間距雷達、低慢小目標(biāo)跟蹤這類場景經(jīng)常是夠用的。6. 把七種變體放到雷達軌跡仿真實例中同臺對比6.1 仿真場景一段帶機動的目標(biāo)軌跡和雷達量測光講每種變體怎么實現(xiàn)還不夠我更想讓你看它們在同一段數(shù)據(jù)上分別是什么表現(xiàn)。所以我構(gòu)造了一個更接近實戰(zhàn)的軌跡目標(biāo)前3秒勻速直線第3到5秒做勻速轉(zhuǎn)彎之后恢復(fù)直線。雷達量測包含20米量級的噪聲采樣率10Hz。這套設(shè)計是故意的勻速段考驗基本跟蹤精度轉(zhuǎn)彎段考驗機動響應(yīng)能力恢復(fù)直線段考驗濾波是否發(fā)散或者過度滯后。6.2 統(tǒng)一測試腳本組織方式為了不重復(fù)寫八套主循環(huán)我在Matlab里用函數(shù)句柄組織每種濾波器的“單步更新邏輯”。核心結(jié)構(gòu)大致是filter_basic (x, P, z, F, H, Q, R) kalman_basic_step(x, P, z, F, H, Q, R); filter_fixed (x, P, z, F, H, Q, R) kalman_fixed_step(x, P, z, F, H, Q, R); % ... 其他同理每種變體都實現(xiàn)成“輸入當(dāng)前狀態(tài)、協(xié)方差、量測輸出更新后的狀態(tài)、協(xié)方差”主循環(huán)只負責(zé)喂數(shù)據(jù)、存結(jié)果。這樣做的好處是以后想加一個新的變體只需要寫一個單步函數(shù)主程序一行都不用改。6.3 實測結(jié)果怎么看誰最先丟目標(biāo)誰最穩(wěn)定我把個人實測的結(jié)論寫在這里可能跟教科書感覺不太一樣基本離散Kalman在勻速段表現(xiàn)不錯但目標(biāo)一進入轉(zhuǎn)彎段誤差立刻拉大轉(zhuǎn)彎結(jié)束后的恢復(fù)也比較慢主要原因是舊數(shù)據(jù)權(quán)重太高。平方根Kalman在數(shù)值穩(wěn)定性上確實強長時間跑下來P矩陣沒有崩但它不解決模型失配問題轉(zhuǎn)彎段誤差依然明顯只是比基本版稍微收斂快一點。遺忘因子Kalman在轉(zhuǎn)彎段的響應(yīng)明顯提升但代價是勻速段的軌跡噪聲變大了。lambda取1.03左右時機動和噪聲之間的平衡還算舒服。自適應(yīng)Kalman在這段數(shù)據(jù)上的綜合表現(xiàn)最好因為量測噪聲波動被R在線估計吸收了一部分。但它的初始化參數(shù)多滑動窗口長度的選擇對結(jié)果影響很大我試了10幀、30幀、50幀收斂速度和穩(wěn)態(tài)精度都不一樣。擴大P Kalman在轉(zhuǎn)彎開始的瞬間能很快拉回目標(biāo)但拉回之后軌跡有明顯過沖如果不加平滑限制過沖會持續(xù)好幾幀。固定增益Kalman在線算力最低但前提是你預(yù)先知道運動模式基本不變。目標(biāo)一轉(zhuǎn)彎固定增益的滯后比基本Kalman還嚴重因為它連P的自適應(yīng)調(diào)整都省了。有限K減小Kalman的曲線介于固定增益和遺忘因子之間穩(wěn)態(tài)精度比固定增益好一些機動響應(yīng)又比全自適應(yīng)差一些但勝在參數(shù)少、調(diào)起來快。我個人的偏好是離線分析用“平方根遺忘因子”組合在線實時平臺用“固定增益有限K減小”組合。自適應(yīng)的理論最強但工程調(diào)試成本也最高項目排期緊的時候謹慎使用。7. 常見問題與排查技巧實錄7.1 發(fā)散現(xiàn)象定位清單濾波發(fā)散是雷達跟蹤里最常遇到的問題我建議按這個順序排查先看P是否對稱正定Matlab里用eig(P)看特征值只要有負特征值基本就是數(shù)值問題直接換Joseph形式或平方根實現(xiàn)。再看新息均值是否為0如果新息長期帶正負號偏置多半是運動模型不對比如目標(biāo)在轉(zhuǎn)彎你還在用勻速模型。最后看NIS是否長期超限NIS偶爾超限可以靠擴大P補救連續(xù)幾百幀超限就不要再補了重新初始化或者切換模型。7.2 Matlab實現(xiàn)層面的高頻報錯我在寫這套代碼時遇到過幾個Matlab特有的坑第一個是矩陣除法。inv(S) * H在數(shù)值上遠不如S \ H或H / S穩(wěn)定。雖然小矩陣在Matlab里看不出差別但狀態(tài)維數(shù)一高inv的精度問題會被放大建議統(tǒng)一寫作P_pred * H / (H * P_pred * H R)。第二個是維度不一致。H * P_pred * H R最容易出維度問題尤其當(dāng)你用diag([20, 20])初始化R但H的定義順序是先方位角后距離的時候矩陣乘法直接報錯。解決方法是逐行檢查size(H)和size(R)。第三個是randn每次運行結(jié)果不一樣。對比多種變體時建議在腳本開頭加rng(2024)固定隨機種子否則每次跑出來的對比曲線都不一樣根本沒法判斷差異是算法造成的還是隨機噪聲造成的。7.3 參數(shù)調(diào)優(yōu)的經(jīng)驗順序如果你面對一套全新的雷達數(shù)據(jù)我的參數(shù)調(diào)優(yōu)順序是第一步先不調(diào)任何參數(shù)用基本離散Kalman配一個大致合理的R跑一遍看量測噪聲量級對不對第二步統(tǒng)計量測新息的樣本協(xié)方差反過來校準R第三步在R固定的前提下從0開始逐漸增大Q直到濾波軌跡的RMSE不再明顯下降第四步如果目標(biāo)有機動段再引入遺忘因子或者自適應(yīng)最后才考慮用擴大P和有限K減小做應(yīng)急兜底。不要一上來就七種變體全開參數(shù)太多之后你根本不知道哪個參數(shù)導(dǎo)致結(jié)果變差。先把基本Kalman調(diào)穩(wěn)再加補丁每一步只改一個變量這樣出了問題才能定位。最后再分享一個我自己的習(xí)慣跑完這七種變體后我不會只看濾波軌跡圖就下結(jié)論。我會把每種變體的RMSE、NIS均值、首次發(fā)散幀數(shù)都打印出來再丟一段帶機動的測試數(shù)據(jù)進去看誰先丟目標(biāo)。雷達軌跡估計沒有“最牛濾波器”只有“最匹配當(dāng)前場景的濾波器”。你手頭那批數(shù)據(jù)到底吃哪一套跑一遍對比比翻十篇論文都有用。后面我準備把這套變體框架往EKF和UKF上再擴一輪到時候再接著分享。