☰
MATLAB卡尔曼滤波飞行轨迹预测:Q/R调参到IMM实践
2026/11/4 3:44:33 网站建设 项目流程

简介:这份面向飞行器导航、目标追踪与信号处理学习者的MATLAB实践资料,围绕卡尔曼滤波实现飞行物体运动轨迹预测,重点解决含噪传感器数据下的状态估计与轨迹平滑问题,适合具备一定概率统计和MATLAB基础、希望理解预测—更新闭环的读者。压缩包共6个文件,约129KB,含3个txt数据与说明、1个m源码、1个docx文档及1个url链接,分别对应飞行数据、经纬度样例、滤波器实现和辅助阅读材料。已有4690人学习下载,说明该主题在相关课程与项目中关注度较高。读者可据此梳理状态转移、测量矩阵、噪声协方差与卡尔曼增益的设置思路,结合20组飞行数据观察位置、速度估计及滤波前后轨迹对比,并保留扩展到EKF、UKF等非线性场景的接口意识,便于后续复现实验与排错。

1. 预测线飘出画面之后,我才发现参数比公式更难

有个做飞行器测控的同行拿了一段数据来找我,地面站上那条预测线在目标进入转弯段 1.5 秒后开始偏,2 秒后直接飞出画面。他第一反应是模型太简单,想换粒子滤波。数据拉出来一看,状态转移矩阵没写错,问题出在过程噪声 q 设成了 1e-6,滤波器几乎完全信任匀速模型,目标一转,它还在按直线往外推。

这份 MATLAB 实现干的就是飞行物体运动轨迹预测这件事:输入一串带噪声的位置量测,输出平滑后的轨迹、下一时刻的预测值,以及一个能画成包络的协方差。它适合做无人机航迹、飞行试验数据处理、课程设计的人,前提是愿意花半小时把 F、H、Q、R 这四样东西的物理含义搞清楚,而不是抄完五个公式就收工。

2. 把飞行目标写成状态空间:F、H、Q、R 四个矩阵怎么定

卡尔曼滤波的代码量其实很小,一个 for 循环里五条公式,一天能默写出来。真正决定结果好坏的是进循环之前的那几个矩阵:状态怎么选、模型取匀速还是匀加速、过程噪声给多大、量测噪声从哪儿来。这一章把四个矩阵逐个拆开,每个都落到「这个数该填多少、为什么」。

2.1 匀速还是匀加速:先看目标会不会「拐」

二维平面追踪最常用两种模型。匀速模型(CV)的状态是[x; vx; y; vy],假设目标速度在两帧之间不变,加速度全部归到过程噪声里;匀加速模型(CA)多带两个加速度分量,状态变成六维,能跟住持续机动的目标,代价是状态维数上去、协方差矩阵变大、对噪声更敏感。

选型不要凭感觉,看两个量:采样周期 T 和目标在 T 内的速度变化量 Δv。如果 Δv 远小于量测噪声对应的速度不确定度,CV 就够;如果目标经常做 3g 以上的转弯,而 T 只有 0.1 s,CV 的滞后会非常明显,预测线会像被橡皮筋拽着一样落在真实轨迹后面。

模型状态维数适用场景主要问题
CV4(二维)巡航段、采样快、机动少转弯段滞后,Q 补偿能力有限
CA6(二维)持续加速、爬升、俯冲加速度不可观时容易振荡
CV + 自适应 Q4机动段占比不高需要额外的机动检测逻辑
IMM(CV+CA)10全程都有,机动模式会切换实现复杂,参数翻倍

新手最常犯的错是「一步到位上 CA」。加速度在只有位置量测的情况下是二阶不可观测量,CA 模型的加速度分量完全靠过程噪声撑起来,Q 给不好就会出现加速度估计在正负之间来回跳,位置反而比 CV 更差。我的习惯是先用 CV 跑通,看残差序列有没有系统性偏置,有偏置再考虑升模型或者上 IMM。

还有一个常被忽略的点:坐标系。屏幕上 y 轴朝下,地图上 y 轴朝北,同一组数据换个画法,转弯方向就反了。建模之前先把坐标系定死,写进注释里,后面所有矩阵都按这个来。

2.2 状态转移矩阵 F 与观测矩阵 H 的推导

CV 模型的物理意义很直白:位置等于上一时刻位置加上速度乘周期,速度不变。写成矩阵就是 F 里位置行对速度列填 T,其余按单位阵排。

F = [1 T 0 0; 0 1 0 0; 0 0 1 T; 0 0 0 1]

观测矩阵 H 描述「传感器能看到状态的哪几维」。只有位置量测时,H 就是一个 2×4 的抽取矩阵,把 x 和 y 挑出来,速度两列全填 0。这一步写错的表现很隐蔽:滤波能跑,轨迹也大致对,但速度估计会缓慢漂移,跑几百帧之后预测值开始系统性偏离。

CA 模型的 F 要多一列 T²/2,形式如下(单轴):

F_axis = [1 T T^2/2; 0 1 T; 0 0 1]

H 对应变成[1 0 0]。注意 T 的单位必须和速度单位一致,位置用米、速度用米每秒、T 用秒,三样东西不能混。我见过把 T 写成毫秒的,F 里的速度项直接放大一千倍,第一帧就发散了。

2.3 过程噪声 Q 与量测噪声 R:一个管「信模型」,一个管「信传感器」

Q 和 R 是整份实现里最像玄学的两个矩阵,但它们各自有明确的物理来源。

R 来自传感器指标,是可以查的。GPS 水平定位精度 3 m,那 R 的位置项就是 9;雷达测距标准差 5 m、测角标准差 0.3°,在 1000 m 处横向误差约 5.2 m,两项合起来构成极坐标下的 R,再转到笛卡尔。R 给得比实际大,滤波器会过度平滑,响应变慢;给得比实际小,量测里的野值会被当成真值吃进去。

Q 来自「模型没描述到的那部分运动」。工程上常用连续白噪声加速度模型,用一个标量 q(单位 m²/s³)乘上噪声增益矩阵。q 的物理含义是目标加速度扰动的功率谱密度:巡航段气流平稳,q 取 0.01 到 0.1;有持续机动的段,q 要上到 1 甚至更高。

参数物理来源给小的后果给大的后果
R传感器标称精度吃野值、轨迹毛刺响应慢、转弯段滞后
q目标机动强度跟不上机动、预测外推噪声放大、预测抖动
P0初始位置/速度不确定度前几十帧收敛慢前几帧剧烈跳变

一个实用技巧:先按传感器手册把 R 定死,不要动它,只调 q。两个一起调,你永远不知道是谁起的作用。

2.4 用 MATLAB 把四个矩阵封装成函数

把矩阵构造单独写成一个函数,主循环里只调用,改参数的时候不用翻代码。下面这份是二维 CV 版本:

function [F, H, Q, R] = build_model(T, q, sigma_pos) % 二维匀速模型,状态顺序 [x; vx; y; vy] % T 采样周期 (s) % q 过程噪声强度 (m^2/s^3) % sigma_pos 位置量测标准差 (m) F = [1 T 0 0; 0 1 0 0; 0 0 1 T; 0 0 0 1]; H = [1 0 0 0; 0 0 1 0]; % 噪声增益矩阵:单位加速度在每个周期内对位置和速度的贡献 G = [T^2/2, 0; T, 0; 0, T^2/2; 0, T]; Q = q * (G * G'); % 4x4,两个轴各自成块 R = diag([sigma_pos^2, sigma_pos^2]); end

G * G'这种写法比手写块对角矩阵不容易错,尤其是升到三维之后。展开后位置项是 T⁴/4、位置速度交叉项是 T³/2、速度项是 T²,和教科书上的离散化结果一致。

参数说明:T 取 0.1 s 对应 10 Hz 更新率;q 从 0.05 起步;sigma_pos 按实际传感器填,不确定就先取量测残差的均方根。三个数确定之后,F、H、Q、R 就固定了,后面所有调参都在这个函数外面做。

3. MATLAB 完整实现:从仿真轨迹到递推滤波的五条公式

矩阵准备好之后,剩下的就是数据流。这一章用一段自己造的航迹把整条链路跑通,包含直线段、圆弧转弯段和出弯后的直线段,这样能同时看到 CV 模型在平稳段和机动段的表现差异。

3.1 造一段会转弯的飞行轨迹

真实数据不好拿,先用参数方程造一条几何上连续的轨迹。转弯段用圆弧,保证位置和速度在切换点连续,否则滤波器会在切换瞬间产生一个假的新息尖峰,看起来像算法有问题,其实是数据自己跳了。

function [Xtrue, Z] = gen_track(T, N, sigma_pos, seed) % 生成「直线 - 圆弧转弯 - 直线」二维航迹 % Xtrue: 4xN 真值 [x; vx; y; vy],Z: 2xN 带噪量测 rng(seed); Xtrue = zeros(4, N); v0 = 180; % 巡航速度 (m/s) w = 0.25; % 转弯角速度 (rad/s) Rc = v0 / w; % 转弯半径 (m) t1 = 8; % 转弯开始 (s) t2 = t1 + (pi/2)/w; % 转弯结束 (s),转 90 度 p0 = [100; 50]; % 初始位置 (m) for k = 1:N tk = (k-1) * T; if tk < t1 Xtrue(:,k) = [p0(1) + v0*tk; v0; p0(2); 0]; elseif tk < t2 th = w * (tk - t1); Xtrue(:,k) = [p0(1) + v0*t1 + Rc*sin(th); v0*cos(th); p0(2) + Rc*(1 - cos(th)); v0*sin(th)]; else px = p0(1) + v0*t1 + Rc; py = p0(2) + Rc; dt = tk - t2; Xtrue(:,k) = [px; 0; py + v0*dt; v0]; end end Z = Xtrue([1 3], :) + sigma_pos * randn(2, N); end

转弯半径 Rc = 180/0.25 = 720 m,转 90° 耗时约 6.28 s。这个量级对应中等机动能力的固定翼目标,q 的初值可以按 0.1 给。

调用方式:T = 0.1; N = 400; [Xtrue, Z] = gen_track(T, N, 3, 7);,400 帧对应 40 秒,前 8 秒直线、中间 6.28 秒转弯、后面出弯直线。seed 固定住,后面调参时结果可比。

3.2 预测与更新:主循环逐行拆

五条公式对应两件事:用模型往前推一步(预测),用量测把推出来的结果拉回来(更新)。下面这份循环里,协方差更新用了 Joseph 形式,原因在避坑那章展开。

[F, H, Q, R] = build_model(T, 0.1, 3); x_est = [Z(1,1); 0; Z(2,1); 0]; % 初始状态:位置取首帧量测 P_est = diag([9, 100^2, 9, 100^2]); % 速度方差先给大 Xhat = zeros(4, N); for k = 1:N % 1) 状态预测 x_pred = F * x_est; % 2) 协方差预测 P_pred = F * P_est * F' + Q; % 3) 新息协方差与增益 S = H * P_pred * H' + R; K = P_pred * H' / S; % 4) 新息与状态更新 innov = Z(:,k) - H * x_pred; x_est = x_pred + K * innov; % 5) 协方差更新(Joseph 形式,保证对称正定) IKH = eye(4) - K * H; P_est = IKH * P_pred * IKH' + K * R * K'; P_est = (P_est + P_est') / 2; % 强制对称 Xhat(:,k) = x_est; end

逐条看。第 1 步只用到模型,跟量测无关;第 2 步把上一帧的不确定度放大,加上过程噪声,体现「推得越远越不准」。第 3 步的 S 是新息的理论方差,K 是「信模型还是信量测」的权重,它的量纲由 P 和 R 共同决定。第 4 步的 innov 是实际量测和预测的差,如果滤波器健康,这个序列应该是零均值白噪声,后面 NIS 检验就是拿它做文章。

第 5 步可以写成(I - K*H) * P_pred,数学上等价,但数值上不等价。减法形式在 K 很小或 P 条件数很差的时候会丢掉对称性,跑几千帧之后 P 出现负特征值,卡尔曼增益直接变成 NaN。Joseph 形式多一次矩阵乘法,换来的是长期稳定性,值得。

初始 P 里速度方差给了 100²,意思是「我不知道目标速度,可能达到 100 m/s 量级」,这是一个很宽的先验。给太小的后果在第 5 章讲。

3.3 Q/R 比例怎么调:用参数扫描代替拍脑袋

调参不要一次改一个数然后盯着图看,做成扫描,用数字说话。下面这段扫 q,R 保持传感器标称值不变:

q_list = [1e-3, 1e-2, 1e-1, 1, 10]; rmse_pos = zeros(size(q_list)); for i = 1:numel(q_list) [F, H, Q, R] = build_model(T, q_list(i), 3); Xh = run_filter(Z, F, H, Q, R); % 封装好的滤波函数 ex = Xh(1,:) - Xtrue(1,:); ey = Xh(3,:) - Xtrue(3,:); rmse_pos(i) = sqrt(mean(ex.^2 + ey.^2)); end disp(table(q_list', rmse_pos', 'VariableNames', {'q', 'RMSE_pos'}));

run_filter就是上一节的循环体,参数化之后直接复用。跑出来一般是一条 U 形曲线:q 太小,转弯段误差堆积,RMSE 高;q 太大,直线段被噪声带偏,RMSE 也高;中间有个谷底。选谷底附近的 q,不要选最小值点,留一点余量给没见过的机动。

要注意 RMSE 的统计口径。这里算的是全体帧的位置误差,转弯段只占十几帧,权重很低,q 会被直线段主导。如果关心机动段表现,把转弯那一段的索引单独拎出来算一次,两个数字一起看。我做参数表的时候习惯分「全程 RMSE」「转弯段 RMSE」「最大瞬时误差」三列,单看一个数很容易选错。

3.4 结果怎么看:轨迹叠加、误差曲线与 3σ 包络

图要画三张,不是一张。第一张轨迹叠加,看宏观形状;第二张误差随时间变化,看有没有系统性偏置;第三张 3σ 包络,看滤波器对自己的不确定度估计准不准。

figure('Position', [100 100 900 700]); subplot(3,1,1); plot(Xtrue(1,:), Xtrue(3,:), 'k-', 'LineWidth', 1.5); hold on; plot(Z(1,:), Z(2,:), '.', 'Color', [0.75 0.75 0.75], 'MarkerSize', 4); plot(Xhat(1,:), Xhat(3,:), 'r-', 'LineWidth', 1.2); legend('真值', '量测', '滤波', 'Location', 'best'); axis equal; grid on; ylabel('Y (m)'); subplot(3,1,2); err = sqrt((Xhat(1,:)-Xtrue(1,:)).^2 + (Xhat(3,:)-Xtrue(3,:)).^2); plot((0:N-1)*T, err, 'b'); grid on; ylabel('位置误差 (m)'); xlabel('时间 (s)'); subplot(3,1,3); sig = zeros(1, N); % 需要把每帧 P 存下来 for k = 1:N sig(k) = 3 * sqrt(P_hist(1,1,k) + P_hist(3,3,k)); end fill([(0:N-1)*T, fliplr((0:N-1)*T)], ... [sig, fliplr(-sig)], [0.8 0.9 1], 'EdgeColor', 'none'); hold on; plot((0:N-1)*T, Xhat(1,:)-Xtrue(1,:), 'b'); grid on; ylabel('X 方向误差 ±3\sigma'); xlabel('时间 (s)');

P_hist需要在主循环里存一份P_est,三维数组P_hist(:,:,k) = P_est。包络的意义在于自检:如果误差曲线大部分时间落在 ±3σ 里面,说明 R 和 Q 的比例大致合理;如果误差频繁顶穿包络,说明滤波器对自己的估计过于自信,R 给小了或者 q 给小了;如果包络宽得能装下整条轨迹,说明参数给得太保守,平滑过度。

转弯段是观察重点。CV 模型在转弯时误差会先上升再回落,上升的幅度直接反映 q 够不够。如果上升之后再也回不来,那大概率是协方差已经失去正定性,去看第 5 章。

4. 量测不理想时:丢帧、极坐标与非线性的补救

实验室里跑通不等于现场能用。真实链路上量测会丢、坐标系会是极坐标、野值会混进来,这三件事各自对应一套处理手法,硬套线性卡尔曼滤波会在这些地方翻车。

4.1 丢帧与异步量测:只预测不更新

量测丢失最直接的处理是跳过更新步,只保留预测。注意不要用上一帧的量测顶替,那等于人为制造了一个「目标没动」的假观测,滤波器会把速度往零拉。

for k = 1:N x_pred = F * x_est; P_pred = F * P_est * F' + Q; if any(isnan(Z(:,k))) x_est = x_pred; % 只预测 P_est = P_pred; Xhat(:,k) = x_est; continue; end S = H * P_pred * H' + R; K = P_pred * H' / S; x_est = x_pred + K * (Z(:,k) - H * x_pred); IKH = eye(4) - K * H; P_est = IKH * P_pred * IKH' + K * R * K'; P_est = (P_est + P_est') / 2; Xhat(:,k) = x_est; end

连续丢帧时 P 会一直涨,涨到一定程度 K 会趋近于「完全信下一次量测」,这时候如果来一个野值,轨迹会被拽飞。工程上的做法是给 P 的对角线设上限,或者连续丢帧超过 N 帧就重置滤波器、用最新量测重新初始化。

异步量测是另一种情况:位置量测 10 Hz,某些辅助量测 1 Hz。不要为了对齐把慢的插值成快的,那会引入相关性。正确做法是把快慢两路当成两个独立的更新步,在同一帧里先做一次预测,然后按到达顺序做两次更新,每次用自己的 H 和 R。

4.2 雷达极坐标量测:EKF 的雅可比怎么推

雷达给的是距离和方位角,状态还在笛卡尔坐标系里,观测方程变成非线性的:

r = sqrt(x^2 + y^2) θ = atan2(y, x)

这时候 H 不再是常数矩阵,而是 h(x) 在预测点处的雅可比。对状态[x; vx; y; vy]求偏导:

H = [ x/r, 0, y/r, 0; -y/r^2, 0, x/r^2, 0]
function [h, Hk] = radar_obs(x) % 极坐标观测方程及其雅可比 px = x(1); py = x(3); r = sqrt(px^2 + py^2); h = [r; atan2(py, px)]; Hk = [ px/r, 0, py/r, 0; -py/r^2, 0, px/r^2, 0]; end

更新时用Hk代替常数 H,其余五条公式不变,这就是 EKF。雅可比必须在预测点处求,不能在更新后的点求,否则协方差传播的顺序就乱了。

极坐标转笛卡尔还有两个坑。一是角度环绕:atan2 的输出在 ±π 之间跳,新息里的角度差必须先归一化到 (-π, π],否则 179° 和 -179° 的差会算成 358°,增益直接爆掉。二是 r 很小时雅可比的条件数极差,目标过顶的时候滤波会抖,这种情况要么切到笛卡尔量测,要么临时把 R 放大。

dth = z(2) - h(2); dth = atan2(sin(dth), cos(dth)); % 角度归一化 innov = [z(1) - h(1); dth];

4.3 用 NIS 检验滤波器有没有「自嗨」

调完参数,图看着挺漂亮,但滤波器可能已经不一致了:它给出的协方差和实际误差对不上。检验办法是归一化新息平方(NIS):

NIS_k = ν_k' * S_k^{-1} * ν_k

ν 是新息,S 是新息协方差。量测是二维时 NIS 服从自由度 2 的卡方分布,95% 门限 5.991,99% 门限 9.210。

nis = zeros(1, N); for k = 1:N % ... 预测步 ... S = H * P_pred * H' + R; nu = Z(:,k) - H * x_pred; nis(k) = nu' / S * nu; % ... 更新步 ... end ratio = mean(nis > 5.991); % 超出 95% 门限的比例

健康的结果是这个比例在 5% 左右。如果到 30% 以上,说明新息比理论预期大,要么 R 给小了,要么 q 给小了,模型跟不上实际运动。如果比例接近 0,说明 R 给大了,滤波器过度平滑,响应会慢。这个指标比看轨迹图靠谱得多,因为轨迹图顺眼不代表统计上正确。

注意 NIS 要在更新之前算,用预测步的 S 和预测步的 x。算在更新之后就没有意义了。

5. 避坑与排查:五条踩过的坑,每条都能让轨迹飞出去

下面这几条都是实际调试里撞过的,症状看起来五花八门,根子大多在数值和参数上。按「现象 → 原因 → 解决」记,方便对号入座。

5.1 协方差失去对称性,几百帧后直接发散

现象:前 200 帧一切正常,之后误差突然指数上升,打印 P 发现有负特征值,K 里出现 NaN。

原因:协方差更新用了(I - K*H) * P_pred的减法形式,浮点误差累积之后 P 不再对称,负特征值被开方后传播到增益里。采样率越高、矩阵条件数越差,出现得越早。

解决:换成 Joseph 形式IKH*P_pred*IKH' + K*R*K',并在每帧末尾强制对称P = (P + P')/2。再加一道保险,用eig(P)检查最小特征值,小于零就重置 P。这几行代码不增加什么计算量,但能省掉一整天的排查。

5.2 P0 给得太小,前 30 帧都在「爬」

现象:轨迹开头一段滤波值几乎贴着初始点不动,量测明明已经跑出去几百米,滤波输出像被粘住了,几十帧之后才追上来。

原因:初始协方差 P0 给得太小,滤波器认为自己「很确定」,于是卡尔曼增益很小,新息被大幅压制。速度分量的初始方差尤其关键,给成 1 意味着「目标速度不会超过 1 m/s」,和实际的 180 m/s 差了三个数量级。

解决:位置方差按首帧量测精度给,速度方差直接给大,100² 甚至 300² 都行,让滤波器自己收敛。收敛过程一般 10 到 20 帧,代价是开头几帧输出跳得厉害,如果下游不能接受,可以加一个 5 帧的预热期,预热期内只输出量测。

5.3 Q 一味调大,预测线抖成锯齿

现象:转弯段确实跟上了,但直线段的滤波轨迹开始出现高频抖动,预测值一帧一个方向,RMSE 反而比调之前更高。

原因:q 调大等于告诉滤波器「模型不可信,多信量测」,量测里的噪声被原样搬进输出。位置噪声 3 m、采样 10 Hz 的情况下,q 超过 1 之后抖动会非常明显。

解决:不要全局调 q,改成按机动状态分段。简单做法是用新息幅值做检测,连续几帧 NIS 超门限就把 q 临时放大 10 倍,恢复正常后再降回去。更规范的做法是 IMM,第 6 章讲。另外,如果量测噪声本身估计偏大,先修 R,别拿 q 去补。

5.4 单位混用:角度、毫秒、像素

现象:滤波器能跑,但增益的量纲看起来不对,速度估计的值大得离谱,或者转弯方向完全相反。

原因:T 用了毫秒但速度按米每秒算;角度用度但 atan2 返回弧度;屏幕坐标 y 轴向下但轨迹按 y 轴向上建模。这三种错误都不会报错,只会让结果慢慢偏掉。

解决:在文件头写一张单位表,所有输入输出都标注单位,函数入口做一次断言。角度统一用弧度,时间统一用秒,坐标系统一用右手系。我现在的习惯是变量名带单位后缀,T_s、sigma_pos_m、theta_rad,写起来啰嗦,但省掉了大量「这个数到底是度还是弧度」的回头确认。

5.5 拿平滑结果当预测精度,评估全错

现象:离线评估 RMSE 只有 0.8 m,现场部署之后预测误差到了十几米,怀疑代码被改了。

原因:评估时用的是 k 时刻的后验估计和 k 时刻的真值比,这是平滑精度,不是预测精度。后验估计已经用到了当前帧量测,而预测场景下只能用到 k-1 帧及之前的信息。

解决:评估必须严格按预报口径做,用 k-1 时刻的估计经过一次预测得到 k 时刻的预测值,再和 k 时刻真值比;或者用 k 时刻的估计去比 k+1 时刻的真值。两种口径的数字会差好几倍,报指标的时候写清楚是哪一种。这一步不做,前面所有调参都是在优化一个不存在的指标。

6. 进阶:IMM 让滤波器自己判断机动,以及怎么验证它真有效

CV 模型加固定 q 的天花板很明显:直线段要平滑就得 q 小,转弯段要跟得紧就得 q 大,两个诉求打架。交互多模型(IMM)的思路是让两个模型并行跑,一个 CV 一个小 q,一个 CA 或 CV 大 q,用似然给每个模型算权重,输出加权结果。机动发生时大 q 模型的权重自动升上去,平稳段再降回来。实现上就是两套预测更新加一步权重更新和状态融合,代码量翻倍,但省掉了手写机动检测的阈值。

% 两个模型的 Markov 转移概率(行:当前,列:下一时刻) Pi = [0.95, 0.05; 0.10, 0.90]; % 每个周期:混合初值 -> 各自预测更新 -> 更新模型概率 -> 融合输出 mu = mu .* likelihood; % likelihood 由新息和 S 算 mu = mu / sum(mu); x_out = Xfilt{1} * mu(1) + Xfilt{2} * mu(2); % 加权融合

验证不能只看一条轨迹。固定随机种子跑 200 次蒙特卡洛,统计位置 RMSE 和 NEES(归一化估计误差平方),NEES 的理论均值等于状态维数,落在卡方区间内说明协方差估计可信。我一般会同时跑 CV、CV 大 q、IMM 三组,把「转弯段最大误差」单独列一列,因为全程 RMSE 会被直线段稀释,看不出机动能力差距。

血泪经验是:IMM 的转移概率 Pi 不要设得太激进,0.05 到 0.1 的量级足够,设成 0.5 会让模型概率高频切换,输出比单模型还抖。另外两个模型的 Q 要拉开至少两个数量级,挨得太近等于白跑。

从那以后我每次交付滤波相关的东西,都强制走一遍「NIS 比例 + 蒙特卡洛 NEES + 预报口径 RMSE」三件套,图好看不算数,统计量对得上才算。希望帮到你。

本文还有配套的精品资源,点击获取

需要专业的网站建设服务?

联系我们获取免费的网站建设咨询和方案报价,让我们帮助您实现业务目标

立即咨询