卡尔曼滤波轨迹预测：状态空间建模、Q/R整定与MATLAB实现

简介面向飞行器导航、目标追踪方向的学习者与工程实践者这份 MATLAB 实现聚焦带噪声测量下的运动轨迹预测用卡尔曼滤波完成状态估计与轨迹修正涉及状态转移矩阵、测量矩阵、过程噪声与测量噪声协方差等核心参数的初始化、预测与更新流程。压缩包共 6 个文件约 129KB以 3 个 txt 数据文件、1 个 m 脚本、1 个 docx 说明文档及 1 个 url 链接为主txt 提供经纬度与飞行测量数据m 文件承载滤波主流程docx 用于补充算法与实验说明。已有 4689 人学习下载说明其在课程设计与入门实践中具有一定参考价值。读者可据此复现从数据读取、预处理到滤波输出与轨迹可视化的完整链路理解卡尔曼增益如何融合预测与观测并对照原始数据评估滤波效果对非线性场景还可顺势延伸到 EKF、UKF 等变体作为飞行控制与导航仿真的起步素材。1. 卡尔曼滤波的真相它预测的不是轨迹是误差该往哪边收飞行物体的轨迹预测在靶场测量、无人机航迹、雷达跟踪里都是刚需用 matlab 写一个卡尔曼滤波器做轨迹预测二十来行就能跑出曲线很多人第一次跑通时都觉得“就这”。但把参数随便一调曲线不是明显滞后就是抖成锯齿于是开始怀疑公式记错了。真正的分水岭不在代码里而在一个认知上卡尔曼滤波每一步只干一件事——把“模型推算”和“传感器观测”这两个都带误差的量按各自的不确定性做加权融合。它不是凭空空想未来而是在持续修正“我该信谁多一点”。搞懂这个加权的比例从哪来残差怎么算滤波器为什么会发散比背五个公式有用得多。2. 状态空间建模为什么要把速度藏进状态向量2.1 状态向量位置之外的量不是装饰飞行目标的运动学描述至少需要位置和速度。只把位置写进状态看起来最贴近传感器输出实际上会让滤波器退化成带延迟的滑动平均。原因很直接位置的一阶差分确实能得到速度但差分把观测噪声放大了 1/dt 倍dt 取 0.05 秒时噪声放大二十倍速度估计基本不可用。卡尔曼滤波的做法是把速度作为“隐状态”一起估计用模型约束把噪声压回去。二维平面匀速模型的状态向量写成四维% 状态 x [px; vx; py; vy]采样周期 dt dt 0.05; F [1 dt 0 0; % px(k1) px vx*dt 0 1 0 0; % vx(k1) vx 0 0 1 dt; % py(k1) py vy*dt 0 0 0 1]; % vy(k1) vyF就是把连续时间的状态方程做前向欧拉离散化得到的状态转移矩阵。匀速模型下这个离散化是精确的因为速度本身不变泰勒展开的高阶项为零。机动目标则需要把加速度也纳入状态或写进过程噪声这一点在第 5 章会展开。状态维度不是越多越好。每多一维协方差矩阵就多一行一列需要整定的参数随之膨胀而未被观测到的维度只能靠模型“猜”一旦模型和真实运动不符多出来的维度会变成误差的存储器。飞行器做轨迹预测时我一般先用四维匀速模型跑通看残差是否还有结构再决定要不要升到六维匀加速或者切换到转弯模型。2.2 协方差矩阵不确定性的形状如果卡尔曼滤波只有一个状态向量那它和普通的递推最小二乘没有本质区别。真正承载“我有多不确定”的是协方差矩阵P。它的对角线是各分量的方差非对角线是分量之间的相关性。一个四维状态P是 4×4 的对称正定矩阵。把位置-速度两维的子块画出来就是一个椭圆长轴方向方差大说明那个方向上估计不可靠椭圆的倾斜角对应位置与速度的相关性。这个形状是有物理含义的。观测只给位置不给速度时位置的不确定性会通过F的耦合传导成速度的不确定性两者天然负相关——位置偏高往往意味着速度被低估。理解了这一点后面“协方差传播”那一步就不会觉得是凭空冒出来的公式。工程上有个容易被忽略的细节P必须在数值上保持对称正定。浮点运算跑上几千步之后P会因为舍入误差慢慢失去对称性极端情况下出现负特征值滤波器当场崩掉。常见的做法是每一步末尾强制对称化P (P P) / 2; % 强制对称成本极低能挡掉大部分数值事故这不是万能解但几乎零成本。2.3 观测矩阵与噪声矩阵H、Q、R 各管什么H描述传感器能看到状态的哪些组合。测距雷达只输出位置那么H就是 2×4H [1 0 0 0; 0 0 1 0]; % 只观测 px 和 py速度不可直接测量H的作用是把四维状态投影到二维观测空间。预测步算出的是状态空间里的先验更新步要把观测和预测拉到同一个空间里比较这个投影就靠H。如果传感器同时输出径向速度H就变成 3×4速度那一列不再是零。H本身不带噪声信息它只回答“测的是什么”。R是观测噪声协方差通常由传感器手册给出也可以在静止条件下采一段数据做样本方差。它衡量的是“传感器有多不靠谱”。Q则是最容易被误解的一个它不是模型误差的方差而是过程噪声——模型没能描述的那部分运动的强度。经典的构造方式是连续白噪声加速度模型假设目标有不相关的随机加速度积分到离散时间域q 9; % 加速度噪声谱密度(m/s^2)^2 * s G [dt^2/2 0; dt 0; 0 dt^2/2; 0 dt]; % 噪声到状态的映射 Q G * diag([q q]) * G; % 4x4量纲自动正确这样构造的Q有个好处位置块和速度块之间的比例由dt自动决定不会出现“位置方差和速度方差随手填两个数”导致的量纲错乱。如果直接把Q写成diag([1 1 1 1])位置和速度的单位不同却给了同样的方差滤波器必然行为怪异。这是新手最常见的翻车点之一。3. 预测与更新五个公式的概率推导3.1 预测步协方差为什么会被“吹胖”预测步干两件事把状态按模型往前推把不确定性放大。xp F * x_est; % 状态外推 Pp F * P * F Q; % 协方差外推 过程噪声状态外推是线性的没什么可说的。协方差那一行来自一个基本结论若随机向量y A x则Cov(y) A Cov(x) A。F是线性的所以先验协方差是F P F。再加Q因为模型本身也有没描述到的运动。这两项一加Pp的行列式几乎必然大于P——不确定性只增不减这就是预测步的物理意义时间每往前走一步你就更不确定目标在哪。这一步解释了滤波器“响应慢”的根源。如果Q给得太小先验协方差增长缓慢更新时算出来的增益也小滤波器更相信模型观测进来只被采纳一点点机动发生时输出就会明显滞后。滞后不是 bug是Q太小的直接后果。3.2 卡尔曼增益一次标量推导看清本质把问题简化到一维、只看位置先验方差是p观测噪声方差是r增益就是K p / (p r)就这么一行。p是先验不确定性r是观测不确定性K是“先验不确定性占总不确定性的比例”。三种极限一眼可知p r模型极不可信、传感器很准K → 1估计几乎完全采纳观测。p r模型很准、传感器很糙K → 0观测几乎被忽略。p r各让一半K 0.5。多维情形下同一个直觉成立只是“比例”变成了矩阵形式的最优权重S H * Pp * H R; % 新息协方差把先验投到观测空间再加观测噪声 K Pp * H / S; % 等价于 Pp*H*inv(S)用 / 求解更稳S是把四维先验协方差投影到二维观测空间、再叠加R得到的结果量纲和观测一致。K的推导来自最小化后验协方差的迹——对trace(P_post)关于K求导并令其为零得到的正是上式。这一步在各类教材里都写得很长但结论的含义只有一句让后验估计的方差最小。注意代码里用的是右除/等价于K Pp*H*inv(S)但走的是线性方程组求解数值上更稳。状态维度不高时差别不大观测维度上去之后差别就明显了。3.3 更新步新息、收缩与 Joseph 形式拿到增益更新就是三步nu z - H * xp; % 新息观测与预测的差 x_est xp K * nu; % 状态修正 IKH eye(4) - K * H; P IKH * Pp * IKH K * R * K; % Joseph 形式更新协方差nu叫新息innovation是整个滤波器里信息量最大的一个量。它表示“观测告诉我的、模型没预料到的那部分”。滤波器正常工作时新息应该是一个零均值、协方差为S的白噪声序列。一旦新息出现趋势、周期性或者方差明显偏离S说明模型和真实运动脱节了。这是排查滤波器问题最直接的观测窗口比看最终轨迹有用得多相当于给滤波器装了个黑匣子。协方差更新这里用的是 Joseph 形式而不是教科书上更常见的P (I - K H) P_pred。两者在数学上等价但后者在K接近最优时有相减操作浮点误差会让P逐渐失去正定性Joseph 形式是三个正定项的和对舍入误差更鲁棒。多写两行代码换来的稳定性长期跑才看得出价值。3.4 非线性化EKF 的雅可比与 UKF 的 sigma 点真实飞行目标的运动很少是匀速直线。转弯、爬升、加速都会让线性模型失效。这时候有两条路。扩展卡尔曼滤波EKF的思路是在当前估计点做一阶泰勒展开用雅可比矩阵替代F和H% 以二维匀速转弯模型为例状态 [x; y; v; theta; omega] % F 的雅可比在当前估计处求值替代固定矩阵 Fk jacobian_f(x_est, dt); Pp Fk * P * Fk Q;EKF 的代价是线性化误差。当状态的不确定性很大、非线性很强时雅可比在均值点求值会引入偏差严重时直接发散。转弯速率估计不准的飞行器是典型受害者。无迹卡尔曼滤波UKF换了个思路不线性化函数而是采样。取 2n1 个 sigma 点让它们精确匹配原分布的均值和协方差把这批点过一遍非线性函数再从不含线性化的结果里重新算均值和协方差。% sigma 点构造n 为状态维数lambda 控制点的散布 lambda alpha^2 * (n kappa) - n; X_sigma [x_est, x_est sqrt(nlambda)*chol(P), x_est - sqrt(nlambda)*chol(P)]; % 逐点过非线性函数后加权还原代价是每步要算 2n1 次函数n6 就是 13 次计算量比 EKF 大一个量级。飞行器跟踪在 matlab 里做离线验证时UKF 的开销完全可以接受上嵌入式平台就得重新算账。选型上我一般的判断是非线性弱、算力紧用 EKF机动剧烈、精度优先用 UKF模型结构都不清楚先老老实实把匀速模型和Q调对往往比换滤波器收益更大。4. MATLAB 实现从零手写一个能跑的跟踪器4.1 造一条带机动的真值轨迹与观测序列验证滤波器之前必须先有真值否则调参就是纯玄学。下面这段生成二维机动轨迹初速给定加速度随时间正弦变化积分两次得到位置然后只对位置叠加高斯噪声当作观测。rng(7); % 固定随机种子保证结果可复现 dt 0.05; T 12; t 0:dt:T; N numel(t); % 真值x/y 方向的机动加速度积分得到速度与位置 ax 3*sin(0.5*t); ay -2*cos(0.3*t); vx cumtrapz(t, ax) 120; % 初速 120 m/s vy cumtrapz(t, ay) 60; x cumtrapz(t, vx); y cumtrapz(t, vy); X_true [x; vx; y; vy]; % 4 x N真值全状态 % 观测只测位置噪声标准差 8 m sigma_r 8; Z X_true([1 3], :) sigma_r * randn(2, N); % 2 x Ncumtrapz用的是梯形积分和F里的前向欧拉离散在dt很小时差别可忽略做验证足够。真值保留完整四维状态是为了能分别评估位置和速度的误差——只看位置误差会掩盖速度估计的崩坏。4.2 主循环从公式到 MATLAB 代码% ---- 模型参数 ---- F [1 dt 0 0; 0 1 0 0; 0 0 1 dt; 0 0 0 1]; H [1 0 0 0; 0 0 1 0]; G [dt^2/2 0; dt 0; 0 dt^2/2; 0 dt]; q 9; % 过程噪声强度(m/s^2)^2*s Q G * diag([q q]) * G; R diag([sigma_r^2, sigma_r^2]); % ---- 初始化位置用首帧观测速度无先验给大方差 ---- x_est [Z(1,1); 0; Z(2,1); 0]; P diag([sigma_r^2, 300^2, sigma_r^2, 300^2]); X_est zeros(4, N); NIS zeros(1, N); for k 1:N % 1) 预测 xp F * x_est; Pp F * P * F Q; % 2) 更新 nu Z(:,k) - H * xp; % 新息 S H * Pp * H R; % 新息协方差 K Pp * H / S; % 卡尔曼增益 x_est xp K * nu; % 后验状态 IKH eye(4) - K * H; P IKH * Pp * IKH K * R * K; % Joseph 形式 P (P P) / 2; % 强制对称 % 3) 记录 X_est(:,k) x_est; NIS(k) nu * (S \ nu); % 归一化新息平方 end初始化那几行值得多说。位置用首帧观测是合理的速度没有任何信息只能给一个很宽的方差——300^2表示“我几乎不知道初速”。这个大方差会让前几步的增益接近 1滤波器快速吸收观测几十步后速度收敛P自然收缩。反过来如果初始化时把速度方差给成1^2滤波器会坚信目标静止前几秒的估计会严重滞后而且这个错误信念要很久才能被观测纠正过来。初始化方差给大不给小是一条值得写进 checklist 的血泪经验。NIS是归一化新息平方下一节要用。4.3 Q 与 R 的整定用 NIS 而不是靠感觉R有传感器手册兜底Q没有。调Q的本质是回答一个问题目标在多大程度上偏离匀速直线运动。给大了滤波器把机动当噪声跟踪紧但输出抖给小了滤波器把机动当异常跟踪平滑但严重滞后。与其反复试数不如看 NIS。理论上nu服从零均值、协方差为S的高斯分布因此nu * S^{-1} * nu服从自由度为观测维数这里是 2的卡方分布。它的 95% 分位数约 5.99dof 2; thr chi2inv(0.95, dof); % ≈ 5.99 ratio mean(NIS thr); % 超阈值比例理想约 5% fprintf(NIS 均值 %.2f理论 %.0f超阈比例 %.1f%%\n, ... mean(NIS), dof, ratio*100);判读规则很直白NIS的均值显著大于 2说明新息比滤波器自认为的更大模型或Q太乐观需要调大Q均值显著小于 2说明滤波器过于保守Q可以再调小。超阈比例远高于 5% 意味着序列不再一致通常伴随机动段——此时应该考虑换模型而不是继续加Q。这套判据比肉眼看曲线可靠因为它看的是滤波器内部自洽性不依赖真值。线上没有真值的场景同样能用。4.4 一致性验证RMSE 之外的判据RMSE 是最直观的指标但它只告诉你“准不准”不告诉你“滤波器是否知道自己在干什么”err_p X_est([1 3], :) - X_true([1 3], :); rmse_p sqrt(mean(err_p(:).^2)); err_v X_est([2 4], :) - X_true([2 4], :); rmse_v sqrt(mean(err_v(:).^2)); fprintf(位置 RMSE %.2f m速度 RMSE %.2f m/s\n, rmse_p, rmse_v);真正的一致性检验要把实际误差和滤波器自己报告的P做对比。如果P的对角线长期大于实际误差平方滤波器过度悲观反过来则是过度自信这种状态最危险因为滤波器自己毫无察觉等到误差爆出来时已经发散。做法是统计归一化估计误差平方NEESNEES zeros(1, N); for k 1:N e X_est(:,k) - X_true(:,k); NEES(k) e * (P_at_k(:, :, k) \ e); % P 需在循环里逐帧存下来 end % 4 维状态NEES 均值应接近 4P要在主循环里额外存一份三维数组开销不大。NEES 均值落在卡方分布置信区间内才说明滤波器的“自信程度”和实际表现匹配。很多看起来 RMSE 很小的滤波器NEES 均值能到几十属于典型的“瞎猫碰上死耗子”换一段数据立刻崩。5. 避坑与排查滤波器翻车的五种现场5.1 估计层面的三个坑坑一轨迹整体滞后机动段尤其明显。现象是估计曲线始终落在真值后面加速度越大滞后越久。原因几乎总是Q给小了先验协方差增长慢增益小观测进来只被采纳一点点。解决办法是把Q按物理量重新推一遍用G*q*G而不是随手填对角阵然后用 NIS 均值确认调到 2 附近。如果调完仍然滞后说明匀速模型不足以描述机动该上匀加速或转弯模型了。坑二输出抖动观测噪声没被压住。现象是估计曲线比真值还毛糙。原因是R给得太小或者Q给得太大增益接近 1滤波器退化成“照抄观测”。解决是先确认R的量纲和数值用静止段数据算样本方差别凭感觉给。另一个隐蔽来源是P的初始化过大前几十步增益一直居高不下等P收缩完曲线才变平滑——这种情况下看前段抖、后段稳属于正常现象不用改参数。坑三滤波器突然发散估计值飞出去。现象是某一帧之后估计值指数级增长。最常见原因是模型和观测的时间戳没对齐dt用错或者观测有丢帧。丢帧时如果直接continueF里仍按一个dt外推误差会累积正确做法是按实际经过的时间构造F或者对缺失的观测跳过更新步只做预测。另一个来源是量纲错误比如角度用度而模型按弧度算。发散一旦发生靠调Q救不回来必须回到数据和模型本身。5.2 工程实现层面的两个坑坑四跑几千步后P变负定程序报错或结果发散。现象是长时间运行后chol(P)失败或者特征值出现负数。原因是协方差更新用了P (I-KH)P的相减形式浮点舍入慢慢破坏正定性。解决是换成 Joseph 形式并在每步末尾做对称化。如果仍然出问题检查R是否严格正定R里有零会直接让S奇异。坑五调好的参数换一段数据就废。现象是在验证集上表现完美换场景立刻崩。原因通常是参数被“拟合”到了特定轨迹上Q的数值没有物理含义纯粹是试出来的。解决办法是回到物理量q的单位是加速度噪声谱密度量级应该和目标可能的机动加速度平方乘时间尺度可比。给q一个能从运动学上说得通的初值再微调换场景的鲁棒性会好一个档次。这件事没有后悔药只能在建模阶段老老实实做。6. 设计取舍调参的本质是分配信任比例卡尔曼滤波所有的“难调”最后都收敛到同一个问题上模型和传感器你更信谁。Q和R的比值决定了这个分配K只是把这个比值翻译成权重。理解了这一点很多看似玄学的现象就都有了解释。参数倾向增益表现输出特征适用场景Q大、R小增益接近 1跟得紧、抖动大强机动、传感器精度高Q小、R大增益接近 0平滑、滞后明显匀速段、观测噪声大两者匹配增益随P自适应机动时收紧、平稳时平滑大多数真实场景这张表最值得看的其实是第三行卡尔曼滤波真正的优势不是某个固定权重而是权重会随不确定性自动变化。平稳段先验协方差小增益自然压低输出平滑机动段新息变大后验协方差修正增益抬升跟踪及时跟上。这个自适应能力是固定系数的低通滤波给不了的。代价是它要求你对噪声有基本判断。Q和R一旦严重失真自适应就变成了自欺——滤波器会非常自信地给出错误答案而且内部指标一切正常。所以我现在的习惯是任何一条跟踪链路在交付前都强制跑一遍 NIS 和 NEES 检验不看曲线好不好看只看滤波器有没有说实话。这个习惯是从一次离线验证全部通过、外场数据一接上就发散的教训里换来的代价不小。希望帮到你。本文还有配套的精品资源点击获取