开始写正文。我最早对卡尔曼滤波产生敬畏感是在某高校实验室做惯性导航系统全球导航卫星系统组合课题那段时间。仿真数据一跑姿态角漂了十几度状态向量越更新越离谱代码却看起来每一步都“符合公式”。后来把整个问题拆成“状态预测”和“量测修正”两条线又在扩展卡尔曼滤波的非线性处理上反复踩坑才总算把那套流程理成了一条顺畅的链路。这篇内容想从三个实际任务——INS/GNSS导航、目标跟踪、地形参考导航——来看卡尔曼滤波器与扩展卡尔曼滤波器在工程里的真实用法。这三个方向经常被当成完全不同的课题但实际上它们共享同一个估计框架差别只在状态定义、量测方程和非线性程度。文里的 MATLAB 代码片段都可以直接带数据跑通适合正在做课程设计、毕业设计或者刚接触组合导航、滤波估计的读者作为参考。1. 三个看似分开的任务最后都回到同一个“预测-修正”回路1.1 任务不同但数学结构高度相似我第一次接触地形参考导航时第一反应是这玩意和惯性导航系统全球导航卫星系统组合差得很远。一个在算位置速度姿态一个在比对数字高程地图还有一个在跟踪雷达目标怎么看都不像一个方向。但等我把三个任务的状态量、量测方程和噪声矩阵摆到同一张表里发现骨架几乎一样。任务典型状态量典型量测量量测-状态关系INS/GNSS导航位置、速度、姿态误差、IMU零偏GNSS位置、GNSS速度弱非线性的坐标换算 姿态误差目标跟踪目标位置、速度可能有机动加速度距离、方位角、俯仰角明显非线性极坐标转直角地形参考导航位置、速度、高度无线电高度表与气压高度之差强非线性依赖地形数据库所谓 Kalman Filter核心只是一个最小方差估计问题你有一个模型负责短时外推有一个量测负责周期性校准两者的不确定性用协方差矩阵表达滤波器做的就是在每一步输出一个“按方差加权以后最靠谱的估计”。INS/GNSS里惯性导航系统负责高频递推GNSS负责低频修正两者的健康状态通过噪声矩阵告诉滤波器目标跟踪里量测模型是从目标直角坐标到雷达极坐标的映射直角坐标状态本身又是我们要估计的东西地形参考导航更直接把高程数据库当成“虚拟量测”的来源用高度差残差反推位置修正量。这个视角如果理顺了后面所有实现细节都能自然找到位置。1.2 模型预测与量测修正的权重分配不少刚上手的人会把 Kalman 滤波器当成简单的“加权平均”其实没有理解它真正在做什么。每一次迭代都分两步预测阶段用系统方程把状态外推一步同时把协方差矩阵增大一点修正阶段把预测值和量测值通过卡尔曼增益做加权。增益公式看起来只是 K P_pred * H / (H * P_pred * H R)但它背后是一套非常直观的决策逻辑当预测协方差P很大、量测噪声R很小的时候滤波器会更信任量测增益会趋近于能完全吃掉新息的数值反过来如果量测噪声很大、模型很可靠增益就会趋于零量测几乎不进状态。这个逻辑用生活例子类比就是开车导航车自己预测接下来五秒的位置通常比较准但如果突然来了一个特别清晰的路况信息你就愿意立刻修改路线如果那个信息来自一个很不靠谱的App你宁可继续按原路走。1.3 非线性才是这三个任务真正的共同门槛线性 Kalman 要求状态转移和量测方程都是线性形式x_pred F * x_prev wz H * x v。但三个任务都不满足。INS/GNSS里有姿态矩阵变换、经纬度到大地方向余弦的转换目标跟踪的雷达量测极坐标和状态直角坐标之间天然是非线性映射地形参考导航的 DEM 高程既是位置的二维非线性函数还可能有剧烈起伏。扩展卡尔曼滤波的基本思路就是在“当前估计值附近”对非线性函数做一阶泰勒展开用雅可比矩阵替代原来的 F 和 H然后继续套用线性滤波公式。思路一句话能讲完但工程实现里的线性化点选择、雅可比正确性、协方差传播一致性每一步都是坑。2. 卡尔曼滤波器与扩展卡尔曼滤波从五个公式到局部线性化2.1 线性卡尔曼的五个公式顺序和维度别记错标准 Kalman 递推只有五个公式但顺序一错就全错。我自己的习惯写法是% 预测 x_prior F * x; P_prior F * P * F Q; % 更新 S H * P_prior * H R; K P_prior * H / S; x x_prior K * (z - H * x_prior); P (eye(n) - K * H) * P_prior;这里有几个初学者特别容易出问题的地方。首先是预测和更新顺序滤波必须在每个量测到来时先执行预测再用当前的量测去修正如果预测和更新的顺序反了协方差和状态会有一拍的时间错位对高频组合导航尤其明显。其次是维度匹配。F 是 n×nH 是 m×nQ 是 n×nR 是 m×m。如果状态里加了 IMU 零偏项却忘记扩充 H 矩阵的对应列Matlab 里矩阵维度会直接报错即使不报错漏一列也会让那个状态永远得不到量测校正慢慢漂出去。最后是 S 矩阵是 m×m 的“新息协方差”K 的公式里要除以 S而不是除以一个标量。有人说在 Matlab 里写成 K P_prior * H * inv(S) 也能跑但数值上直接除会更稳一些尤其是在 S 接近奇异的时候。2.2 扩展卡尔曼先求导再进标准框架EKF 与 KF 的差别只在于两个地方状态转移矩阵 F 变成了函数 f(x) 对状态 x 的雅可比矩阵量测矩阵 H 变成了函数 h(x) 对状态 x 的雅可比矩阵。其余预测和更新的写法完全一样。但是“雅可比”三个字容易让人产生错觉以为只要会求导就能写好 EKF。真正的问题在于线性化点在哪里、线性化误差控制到什么程度、协方差传播时用哪一时刻的雅可比。严格做法是预测阶段用当前最优估计处的 Fk 传播协方差更新阶段用预测状态处的 Hk 计算增益。很多人图省事全程用一个初始点算出的常数矩阵结果滤波器在非线性较强的地方直接失真。一个直观例子对于地形参考导航高程函数 h_terrain(x, y) 在地形陡峭处的一阶导数可能变化非常快如果仍然用初始点的固定梯度去近似滤波器会把位置估计推到完全错误的山坡上。2.3 初始协方差 P0 的意义告诉滤波器“你对自己有多了解”P0 不是随便设的小矩阵它代表的是初始时刻对状态估计误差的置信度。组合导航里面如果初始位置来自普通民用GPS水平位置误差可能十几米那么 P0 里对应位置误差的方差就应设成十几米的平方而不是给一个非常小的数值。我见过最快的发散方式就是把 P0 设成一个接近零的矩阵然后告诉滤波器“我的初始状态绝对准确”。这样一来第一个量测到来时增益几乎为0新息完全进不去等协方差慢慢被 Q 顶起来状态可能已经跑偏了。正确做法反过来初始协方差宁可给大一点让滤波器在前几步快速收敛。3. INS/GNSS组合导航15维误差状态与大地坐标的衔接3.1 为什么要用误差状态而不是全状态惯性导航系统本身在递推位置、速度、姿态GNSS提供绝对位置和速度参考。做组合导航时一个很自然的选择是把完整的导航状态放进滤波器但工程上更常见的是把“惯性导航系统相对于真实值的误差”作为滤波状态。全状态滤波的问题是姿态角、速度、位置的数量级差异很大而且姿态方程有很强的非线性滤波器数值稳定性容易变差。误差状态滤波则利用了“惯性导航系统的短期递推误差是慢变的”这一特点误差量级远小于全量线性化精度更高也便于把陀螺零偏、加速度计零偏这类误差源直接和位置误差、姿态误差建立联系。经典的松耦合惯性导航系统全球导航卫星系统方案用15维状态位置误差3维、速度误差3维、姿态误差3维、陀螺零偏3维、加速度计零偏3维。之所以把零偏也放进去是因为陀螺零偏和姿态误差之间存在强耦合——不估计零偏姿态误差就没法收敛位置误差也会二次方增长。3.2 坐标系转换比滤波公式更优先的工作做惯性导航系统全球导航卫星系统仿真的第一件事不是写滤波循环而是把坐标系定义清楚。仿真里惯导通常跑在东北天ENU或北东地NED坐标下而GNSS原始输出往往基于地心地固ECEF坐标卫星位置和伪距都要先在 WGS-84 椭球模型下完成坐标换算。我见过一个最隐蔽的bug滤波器内部状态用的是经纬度高程量测来自一个ECEF坐标解算结果中间只差一步坐标转换没有做结果协方差矩阵在几次更新后迅速失去正定性Matlab直接提示矩阵接近奇异。后来我用一个静止点的数据先做开环验证才发现位置量测一直带着一个巨大的常值偏差。正确流程是先用lla2ecef或类似函数把量测转到 ECEF再通过姿态矩阵转到导航坐标系最后进入滤波器的 H 矩阵设计。很多教程里把 H 直接写成 [0_{3×3}, I_{3×3}, 0_{3×9}]那是在“量测已经位于滤波坐标系”的假设下成立的实际数据未必满足这个前提。3.3 松耦合量测方程的构造松耦合模式下GNSS 定位结果被当成带噪声的位置和速度量测与惯性推导值比较生成新息。位置量测方程可以写成z_pos pos_ins H_pos * δx v_pos其中 H_pos 对应状态向量里的位置误差分量速度同理。代码上通常这么组织% 状态量δL, δλ, δh, δvE, δvN, δvU, φE, φN, φU, εg(3), ∇a(3) % 如果滤波器按 NED 坐标系写位置误差项和速度误差项的位置要对齐 H_pos [zeros(3, 3), eye(3), zeros(3, 9)]; H_vel [zeros(3, 6), eye(3), zeros(3, 6)]; Hk [H_pos; H_vel]; innov [gnss_pos - ins_pos; gnss_vel - ins_vel]; S Hk * P_prior * Hk Rk; K P_prior * Hk / S; dx K * innov;这里必须注意ins_pos 和 ins_vel 应该是惯导递推在 GNSS 量测对应时刻的值而不是滤波器上一时刻的估计值。如果两者时间基准没对齐新息里就混入了运动状态本身的差值滤波结果会被带偏。工程上会用“惯导外推到PPS时刻”的方法处理时间同步仿真里则通常认为传感器数据已经在同一时间戳上生成但这一步的接口设计仍然值得单独写清楚。3.4 我第一次跑 INS/GNSS 仿真时遇到的速度发散最让我印象深刻的调试经历是跑一个直线匀速的模拟轨迹时位置估计一开始收敛得很好但速度估计在某个时刻开始持续偏离位置误差也跟着涨回来。检查过程噪声矩阵发现我把陀螺零偏的过程噪声设得过大导致滤波器以为零偏每时每刻都在剧烈变化于是把位置和速度的修正量大量分配到零偏上反而牺牲了速度估计。这个问题的本质是状态可观测性问题某些状态下量测并不能唯一确定误差来源。GNSS只提供位置和速度姿态误差和IMU零偏是弱可观测的只有载体做机动时姿态误差才通过比力方程被激励起来。如果过程噪声设置不当滤波器会做出“看似合理但物理上错误”的误差分配。调参建议陀螺零偏和加速度计零偏对应的过程噪声应该反映真实的物理漂移特性而不是用于吸收滤波器的数值误差。零偏随机游走方差设得过大会让估计噪声变大设得过小则零偏收敛速度变慢通常先用 Allan 方差或器件手册做初值再在仿真中微调。4. 目标跟踪里的EKF极坐标量测到直角坐标状态的雅可比矩阵4.1 目标模型选择匀速、匀加速还是匀速转弯目标跟踪的常用做法是假设目标在短时间内满足某个运动模型。匀速模型CV把状态定为 [x, y, vx, vy]状态方程是常速度外推匀加速模型CA再加上加速度项匀速转弯模型CT把转弯率也放进状态里。工程里没有哪个模型能永远描述真实目标。直线目标用 CT 模型跟踪会浪费参数转弯目标用 CV 模型跟踪则新息里会长期携带一个非零的系统偏差。解决思路是设置多个模型并行滤波再用概率权重做交互IMM或者简单一点在滤波器中把过程噪声设置得足够大让模型具备一定机动适应能力。对初学者我建议先把 CV 模型跑透再对比 CA 和 CT 的结果。CV 模型下的状态转移矩阵简单直观也容易把雅可比矩阵和量测处理单独拆出来验证。4.2 极坐标到直角坐标的量测雅可比雷达通常直接输出距离 r、方位角 θ有时还带俯仰角 φ。目标状态如果是直角坐标 [x, y, vx, vy]那么量测方程就是z_pred [sqrt(x^2 y^2); atan2(y, x)]这个 h(x) 对 x 的雅可比矩阵是很多同学第一次求导卡住的地方。直接写出来H [ x/r, y/r, 0, 0; -y/(r^2), x/(r^2), 0, 0 ]其中 r sqrt(x^2 y^2)。在代码里可以这样构造r sqrt(x^2 y^2); H [ x/r, y/r, 0, 0; -y/(r^2), x/(r^2), 0, 0];注意第二行是对方位角的偏导量纲是 1/米反映的是“目标横向移动一个单位方位角变化多少”。这个矩阵如果数值上有数量级差距很大的元素EKF 可能在更新时出现数值不稳定尤其当目标几乎在雷达正前方时方位角对 y 的敏感度会变得特别大。4.3 距离、方位角的噪声协方差 R雷达量测噪声通常是在极坐标空间给出的距离噪声 σ_r角度噪声 σ_θ。滤波器的 R 矩阵就是 diag(σ_r^2, σ_θ^2)。一个常见问题是用直角坐标系下的噪声想想替代极坐标量测的噪声结果模型失真。还有一个更隐蔽的问题是极坐标测量误差虽然在小误差前提下能近似成直角坐标的椭圆分布但目标距离越远同一个角度误差对应的横向位置误差就越大。EKF 的雅可比矩阵天然会把这种距离相关的不确定性通过 H P H R 体现出来所以只要状态和量测的框架正确这个效果是自动获得的。4.4 目标跟踪里最容易出现的“滤波发散”和应对目标跟踪发散有两个典型原因。一是过程噪声给得太小目标一机动新息长期超限但滤波器置信度又很高增益被压得极低状态更新几乎停滞最后轨迹脱靶。解决办法是适当增大 Q或使用自适应方法比如根据新息实际统计值动态调整 Q。二是量测异常值没有预处理。雷达会偶尔冒出野值这些值进入滤波器后会让状态瞬间跳出一个很大的幅度协方差虽然之后会压缩回去但已经污染了航迹。工程上普遍做法是做一个“新息卡方检验”计算 d innov / S * innov如果 d 超过某个阈值比如对应95%置信度的卡方分布临界值就认为这个量测不合格跳过本次更新只做预报。d innov * inv(S) * innov; if d chi2inv(0.95, length(innov)) % 正常更新 else % 野值仅保留预测结果 end5. 地形参考导航高程匹配与非线性地形模型的滤波策略5.1 基本原理两个高度表之差就是一次“地形量测”地形参考导航的思路可以概括成一句话飞行器又装气压高度表又装无线电高度表。气压高度得到的是相对海拔高度的估计无线电高度得到的是相对地面高度的估计。二者相减就在理论上得到了当前所在地面点的海拔高程。问题是这个“测得的高程”无法直接定位因为它没有参考点。但飞行器又带有一块数字高程数据库只要把某个候选水平位置 (x, y) 输入 DEM就能得到“如果我真在这里理论高程应该是多少”。将理论高程与实际测量高程做差残差越小说明候选点越可能是真实位置。这个过程本质上是把位置估计变成一个非线性最优匹配问题。5.2 用 EKF 吸收 DEM 提供的“高程伪量测”我可以把状态选为水平位置、速度以及气压高度计误差等。量测方程写为z_pred h_dem(x, y) h_bias其中 h_dem 是从数字高程数据库中读取的插值高程。它对状态 x 和 y 的偏导就是 DEM 在候选点处的局部坡度这一步需要数值差分或者对 DEM 做平滑插值以后再求导。这里有一个重要的实现细节DEM 数据是离散格网直接读取会得到台阶状的高程函数导数在格点时变得不稳定。因此实际代码里通常用双线性插值或双三次插值生成连续高程面再对插值函数求导。如果不做插值而直接查询格点值EKF 的雅可比矩阵会出现频繁突变滤波协方差很难收敛。5.3 平坦地形上的可观测性灾难如果飞行器飞过一片近乎平坦的平原那么任何水平位置候选点给出的大致是相近的高程。量测残差的差异将变得非常小水平位置的观测量几乎为零。这会造成 EKF 的水平位置协方差无法收缩滤波器退化为纯惯性递推模式位置误差随时间缓慢增长。这种情况不是滤波算法错了而是任务本身的观测信息不足。工程上的应对包括充分利用其他时刻/其他地形特征积累信息或者在定位质量评估中标记当前“地形信息度”低切换到其他导航源。5.4 多假设与粒子化思想EKF 在线性化点偏离真实位置较远时DEM 的实际非线性会让滤波修正量走入错误方向甚至收敛到错误的山谷。解决思路之一是“多点假设”在当前位置附近生成一组候选格网点每个格网点的位置计算其高程残差和似然概率保留高概率点作为新的滤波中心。对应 MATLAB 实现的基本结构大致是% 候选点集合在预测位置周围生成 for i 1:length(candidates) h_theory interp2(DEM_x, DEM_y, DEM, x_c(i), y_c(i), cubic); residual h_measured - h_theory; likelihood(i) exp(-0.5 * residual^2 / sigma_h^2); end likelihood likelihood / sum(likelihood); % 用概率对候选点加权得到本次有效修正这种方法在实践里比纯 EKF 稳健得多尤其适合地形起伏大、初始位置误差大的场景。很多地形参考导航论文里的“最大似然法”“相关定位法”本质上都是这个框架。5.5 地形参考导航的初始化问题地形参考导航有一个前提取条件初始水平位置误差必须足够小否则搜索空间太大候选点数量爆炸匹配结果可能出现多解。工程上通常在进入地形参考导航模式之前先用惯性导航系统全球导航卫星系统把位置误差压制到几百米以内再开始地形匹配。我在仿真里习惯分两阶段第一阶段模拟“预定位”只用惯导递推位置配合卫星量测收敛误差第二阶段关掉卫星量测模拟卫星拒止环境只依靠 DEM 高度差维持位置估计。这样设计更容易观察地形参考导航在“无卫星但有地形起伏”条件下的真实定位能力和发散速度。6. MATLAB实现中的参数调整与调试经验6.1 协方差矩阵的初值试凑顺序很多第一次上手的人一上来就去调 Q 和 R其实一个更合理的顺序是固定量测噪声 R 为传感器手册或经验值。用一个简单的直线轨迹跑通滤波确认位置收敛。再逐渐减小 Q观察是否出现“协方差收缩过快、后续新息进不来”的问题。最后在转弯或机动轨迹上验证 Q 是否足够适应动态变化。这个顺序的核心是先把状态估计问题跑通再处理动态调节问题。如果直接从机动轨迹开始调参几个参数会相互纠缠非常难定位问题。6.2 R 和 Q 的量纲必须一致Q 是过程噪声R 是量测噪声它们都会通过状态方程的量纲影响滤波增益。有人调参数时会面对一个很奇怪的现象位置估计还算正常但速度估计噪声很大原因是 Q 矩阵里速度噪声对应项的物理含义没有和 R 对齐。举个简单例子如果量测是位置量测噪声 R 的量纲是 m²那过程噪声 Q 中对应速度的项就应该通过“速度噪声 × 时间步长后变为位置波动”这个关系来设定而不是直接拍一个数字。一个常用经验是先用误差传播模型把 Q 从传感器噪声等级推导出来再在仿真里放大或缩小二到三倍调整收敛速率。6.3 解析雅可比与数值雅可比的选择EKF 实现里雅可比矩阵既可以用手推的解析式也可以用数值差分近似得到。解析雅可比速度快、精度高但每个任务都要重新推导而且推导错了很难查。数值雅可比用(f(xdx)-f(x-dx))/(2*dx)中心差分近似几乎不需要手推公式但步长 dx 如果选得不好结果会有较大误差。我的经验是复杂量测模型尤其是地形参考导航里的 DEM 插值函数先用数值雅可比跑通算法确认滤波不发散以后再用手推的解析雅可比优化速度并对比两者输出。这样既兼顾开发效率也保证最终实现更稳健。6.4 利用新息序列检查滤波器健康度滤波器的“健康感”可以通过新息序列来观察正常情况下的新息应该是一个零均值、方差基本稳定的序列。如果新息一直同号说明状态里有系统偏差或模型误差如果新息随机但方差特别大说明 R 被低估了实际量测并没有滤波器认为的那么可靠。我调试 INS/GNSS 组合时会把新息直接画出来看位置新息里如果出现锯齿形状基本可以断定是时间同步问题。目标跟踪里如果新息始终偏向同一个方向则大概率是目标模型偏离了真实运动模式。地形参考导航里新息相关性最强因为 DEM 高程本身有空间相关性处理时要结合地形尺度考虑。6.5 从仿真数据过渡到实测数据时的三个改动仿真代码里的理想量测噪声常常是高斯白噪声但实测数据里的误差还有偏差、温漂、异常值和时延。从仿真过渡到实测时我一般先做三件事给每个量测通道单独做离线统计分析确认噪声方差和是否存在常值偏差把量测时间戳和惯导递推时间戳做成对齐最后再加野值剔除逻辑。很多人忽略常值偏差。惯导本身如果初始对准不充分姿态里就带一个小常值偏角GNSS 定位在某些遮挡环境下也有系统偏离。这些偏差如果不补偿EKF 会试图用零偏状态去吸收它最终零偏和位置误差被错误耦合误差特性完全失真。写在代码之外如果只记住一句话那就是卡尔曼滤波器本身给不了你模型但它会在你已经具备的模型基础上用最优统计方式组合量测。INS/GNSS 组合是否稳定常见取决于误差状态设计目标跟踪是否精准往往取决于过程噪声模型匹配实际机动地形参考导航能否锁定位置最依赖地形特征和初始化质量。我自己反复折腾下来的体会是先把标准线性卡尔曼在二维目标跟踪里彻底跑明白把预测、更新、新息检验这些基本功练扎实再去上手扩展卡尔曼滤波的雅可比推导最后进入惯性导航系统全球导航卫星系统组合和地形参考导航会顺很多。MATLAB 的价值在于能把调试周期压缩到很短唯一需要的是像做实验一样去假设、验证、修改假设而不是对着公式空转。
开始写正文。我最早对卡尔曼滤波产生敬畏感是在某高校实验室做惯性导航系统全球导航卫星系统组合课题那段时间。仿真数据一跑姿态角漂了十几度状态向量越更新越离谱代码却看起来每一步都“符合公式”。后来把整个问题拆成“状态预测”和“量测修正”两条线又在扩展卡尔曼滤波的非线性处理上反复踩坑才总算把那套流程理成了一条顺畅的链路。这篇内容想从三个实际任务——INS/GNSS导航、目标跟踪、地形参考导航——来看卡尔曼滤波器与扩展卡尔曼滤波器在工程里的真实用法。这三个方向经常被当成完全不同的课题但实际上它们共享同一个估计框架差别只在状态定义、量测方程和非线性程度。文里的 MATLAB 代码片段都可以直接带数据跑通适合正在做课程设计、毕业设计或者刚接触组合导航、滤波估计的读者作为参考。1. 三个看似分开的任务最后都回到同一个“预测-修正”回路1.1 任务不同但数学结构高度相似我第一次接触地形参考导航时第一反应是这玩意和惯性导航系统全球导航卫星系统组合差得很远。一个在算位置速度姿态一个在比对数字高程地图还有一个在跟踪雷达目标怎么看都不像一个方向。但等我把三个任务的状态量、量测方程和噪声矩阵摆到同一张表里发现骨架几乎一样。任务典型状态量典型量测量量测-状态关系INS/GNSS导航位置、速度、姿态误差、IMU零偏GNSS位置、GNSS速度弱非线性的坐标换算 姿态误差目标跟踪目标位置、速度可能有机动加速度距离、方位角、俯仰角明显非线性极坐标转直角地形参考导航位置、速度、高度无线电高度表与气压高度之差强非线性依赖地形数据库所谓 Kalman Filter核心只是一个最小方差估计问题你有一个模型负责短时外推有一个量测负责周期性校准两者的不确定性用协方差矩阵表达滤波器做的就是在每一步输出一个“按方差加权以后最靠谱的估计”。INS/GNSS里惯性导航系统负责高频递推GNSS负责低频修正两者的健康状态通过噪声矩阵告诉滤波器目标跟踪里量测模型是从目标直角坐标到雷达极坐标的映射直角坐标状态本身又是我们要估计的东西地形参考导航更直接把高程数据库当成“虚拟量测”的来源用高度差残差反推位置修正量。这个视角如果理顺了后面所有实现细节都能自然找到位置。1.2 模型预测与量测修正的权重分配不少刚上手的人会把 Kalman 滤波器当成简单的“加权平均”其实没有理解它真正在做什么。每一次迭代都分两步预测阶段用系统方程把状态外推一步同时把协方差矩阵增大一点修正阶段把预测值和量测值通过卡尔曼增益做加权。增益公式看起来只是 K P_pred * H / (H * P_pred * H R)但它背后是一套非常直观的决策逻辑当预测协方差P很大、量测噪声R很小的时候滤波器会更信任量测增益会趋近于能完全吃掉新息的数值反过来如果量测噪声很大、模型很可靠增益就会趋于零量测几乎不进状态。这个逻辑用生活例子类比就是开车导航车自己预测接下来五秒的位置通常比较准但如果突然来了一个特别清晰的路况信息你就愿意立刻修改路线如果那个信息来自一个很不靠谱的App你宁可继续按原路走。1.3 非线性才是这三个任务真正的共同门槛线性 Kalman 要求状态转移和量测方程都是线性形式x_pred F * x_prev wz H * x v。但三个任务都不满足。INS/GNSS里有姿态矩阵变换、经纬度到大地方向余弦的转换目标跟踪的雷达量测极坐标和状态直角坐标之间天然是非线性映射地形参考导航的 DEM 高程既是位置的二维非线性函数还可能有剧烈起伏。扩展卡尔曼滤波的基本思路就是在“当前估计值附近”对非线性函数做一阶泰勒展开用雅可比矩阵替代原来的 F 和 H然后继续套用线性滤波公式。思路一句话能讲完但工程实现里的线性化点选择、雅可比正确性、协方差传播一致性每一步都是坑。2. 卡尔曼滤波器与扩展卡尔曼滤波从五个公式到局部线性化2.1 线性卡尔曼的五个公式顺序和维度别记错标准 Kalman 递推只有五个公式但顺序一错就全错。我自己的习惯写法是% 预测 x_prior F * x; P_prior F * P * F Q; % 更新 S H * P_prior * H R; K P_prior * H / S; x x_prior K * (z - H * x_prior); P (eye(n) - K * H) * P_prior;这里有几个初学者特别容易出问题的地方。首先是预测和更新顺序滤波必须在每个量测到来时先执行预测再用当前的量测去修正如果预测和更新的顺序反了协方差和状态会有一拍的时间错位对高频组合导航尤其明显。其次是维度匹配。F 是 n×nH 是 m×nQ 是 n×nR 是 m×m。如果状态里加了 IMU 零偏项却忘记扩充 H 矩阵的对应列Matlab 里矩阵维度会直接报错即使不报错漏一列也会让那个状态永远得不到量测校正慢慢漂出去。最后是 S 矩阵是 m×m 的“新息协方差”K 的公式里要除以 S而不是除以一个标量。有人说在 Matlab 里写成 K P_prior * H * inv(S) 也能跑但数值上直接除会更稳一些尤其是在 S 接近奇异的时候。2.2 扩展卡尔曼先求导再进标准框架EKF 与 KF 的差别只在于两个地方状态转移矩阵 F 变成了函数 f(x) 对状态 x 的雅可比矩阵量测矩阵 H 变成了函数 h(x) 对状态 x 的雅可比矩阵。其余预测和更新的写法完全一样。但是“雅可比”三个字容易让人产生错觉以为只要会求导就能写好 EKF。真正的问题在于线性化点在哪里、线性化误差控制到什么程度、协方差传播时用哪一时刻的雅可比。严格做法是预测阶段用当前最优估计处的 Fk 传播协方差更新阶段用预测状态处的 Hk 计算增益。很多人图省事全程用一个初始点算出的常数矩阵结果滤波器在非线性较强的地方直接失真。一个直观例子对于地形参考导航高程函数 h_terrain(x, y) 在地形陡峭处的一阶导数可能变化非常快如果仍然用初始点的固定梯度去近似滤波器会把位置估计推到完全错误的山坡上。2.3 初始协方差 P0 的意义告诉滤波器“你对自己有多了解”P0 不是随便设的小矩阵它代表的是初始时刻对状态估计误差的置信度。组合导航里面如果初始位置来自普通民用GPS水平位置误差可能十几米那么 P0 里对应位置误差的方差就应设成十几米的平方而不是给一个非常小的数值。我见过最快的发散方式就是把 P0 设成一个接近零的矩阵然后告诉滤波器“我的初始状态绝对准确”。这样一来第一个量测到来时增益几乎为0新息完全进不去等协方差慢慢被 Q 顶起来状态可能已经跑偏了。正确做法反过来初始协方差宁可给大一点让滤波器在前几步快速收敛。3. INS/GNSS组合导航15维误差状态与大地坐标的衔接3.1 为什么要用误差状态而不是全状态惯性导航系统本身在递推位置、速度、姿态GNSS提供绝对位置和速度参考。做组合导航时一个很自然的选择是把完整的导航状态放进滤波器但工程上更常见的是把“惯性导航系统相对于真实值的误差”作为滤波状态。全状态滤波的问题是姿态角、速度、位置的数量级差异很大而且姿态方程有很强的非线性滤波器数值稳定性容易变差。误差状态滤波则利用了“惯性导航系统的短期递推误差是慢变的”这一特点误差量级远小于全量线性化精度更高也便于把陀螺零偏、加速度计零偏这类误差源直接和位置误差、姿态误差建立联系。经典的松耦合惯性导航系统全球导航卫星系统方案用15维状态位置误差3维、速度误差3维、姿态误差3维、陀螺零偏3维、加速度计零偏3维。之所以把零偏也放进去是因为陀螺零偏和姿态误差之间存在强耦合——不估计零偏姿态误差就没法收敛位置误差也会二次方增长。3.2 坐标系转换比滤波公式更优先的工作做惯性导航系统全球导航卫星系统仿真的第一件事不是写滤波循环而是把坐标系定义清楚。仿真里惯导通常跑在东北天ENU或北东地NED坐标下而GNSS原始输出往往基于地心地固ECEF坐标卫星位置和伪距都要先在 WGS-84 椭球模型下完成坐标换算。我见过一个最隐蔽的bug滤波器内部状态用的是经纬度高程量测来自一个ECEF坐标解算结果中间只差一步坐标转换没有做结果协方差矩阵在几次更新后迅速失去正定性Matlab直接提示矩阵接近奇异。后来我用一个静止点的数据先做开环验证才发现位置量测一直带着一个巨大的常值偏差。正确流程是先用lla2ecef或类似函数把量测转到 ECEF再通过姿态矩阵转到导航坐标系最后进入滤波器的 H 矩阵设计。很多教程里把 H 直接写成 [0_{3×3}, I_{3×3}, 0_{3×9}]那是在“量测已经位于滤波坐标系”的假设下成立的实际数据未必满足这个前提。3.3 松耦合量测方程的构造松耦合模式下GNSS 定位结果被当成带噪声的位置和速度量测与惯性推导值比较生成新息。位置量测方程可以写成z_pos pos_ins H_pos * δx v_pos其中 H_pos 对应状态向量里的位置误差分量速度同理。代码上通常这么组织% 状态量δL, δλ, δh, δvE, δvN, δvU, φE, φN, φU, εg(3), ∇a(3) % 如果滤波器按 NED 坐标系写位置误差项和速度误差项的位置要对齐 H_pos [zeros(3, 3), eye(3), zeros(3, 9)]; H_vel [zeros(3, 6), eye(3), zeros(3, 6)]; Hk [H_pos; H_vel]; innov [gnss_pos - ins_pos; gnss_vel - ins_vel]; S Hk * P_prior * Hk Rk; K P_prior * Hk / S; dx K * innov;这里必须注意ins_pos 和 ins_vel 应该是惯导递推在 GNSS 量测对应时刻的值而不是滤波器上一时刻的估计值。如果两者时间基准没对齐新息里就混入了运动状态本身的差值滤波结果会被带偏。工程上会用“惯导外推到PPS时刻”的方法处理时间同步仿真里则通常认为传感器数据已经在同一时间戳上生成但这一步的接口设计仍然值得单独写清楚。3.4 我第一次跑 INS/GNSS 仿真时遇到的速度发散最让我印象深刻的调试经历是跑一个直线匀速的模拟轨迹时位置估计一开始收敛得很好但速度估计在某个时刻开始持续偏离位置误差也跟着涨回来。检查过程噪声矩阵发现我把陀螺零偏的过程噪声设得过大导致滤波器以为零偏每时每刻都在剧烈变化于是把位置和速度的修正量大量分配到零偏上反而牺牲了速度估计。这个问题的本质是状态可观测性问题某些状态下量测并不能唯一确定误差来源。GNSS只提供位置和速度姿态误差和IMU零偏是弱可观测的只有载体做机动时姿态误差才通过比力方程被激励起来。如果过程噪声设置不当滤波器会做出“看似合理但物理上错误”的误差分配。调参建议陀螺零偏和加速度计零偏对应的过程噪声应该反映真实的物理漂移特性而不是用于吸收滤波器的数值误差。零偏随机游走方差设得过大会让估计噪声变大设得过小则零偏收敛速度变慢通常先用 Allan 方差或器件手册做初值再在仿真中微调。4. 目标跟踪里的EKF极坐标量测到直角坐标状态的雅可比矩阵4.1 目标模型选择匀速、匀加速还是匀速转弯目标跟踪的常用做法是假设目标在短时间内满足某个运动模型。匀速模型CV把状态定为 [x, y, vx, vy]状态方程是常速度外推匀加速模型CA再加上加速度项匀速转弯模型CT把转弯率也放进状态里。工程里没有哪个模型能永远描述真实目标。直线目标用 CT 模型跟踪会浪费参数转弯目标用 CV 模型跟踪则新息里会长期携带一个非零的系统偏差。解决思路是设置多个模型并行滤波再用概率权重做交互IMM或者简单一点在滤波器中把过程噪声设置得足够大让模型具备一定机动适应能力。对初学者我建议先把 CV 模型跑透再对比 CA 和 CT 的结果。CV 模型下的状态转移矩阵简单直观也容易把雅可比矩阵和量测处理单独拆出来验证。4.2 极坐标到直角坐标的量测雅可比雷达通常直接输出距离 r、方位角 θ有时还带俯仰角 φ。目标状态如果是直角坐标 [x, y, vx, vy]那么量测方程就是z_pred [sqrt(x^2 y^2); atan2(y, x)]这个 h(x) 对 x 的雅可比矩阵是很多同学第一次求导卡住的地方。直接写出来H [ x/r, y/r, 0, 0; -y/(r^2), x/(r^2), 0, 0 ]其中 r sqrt(x^2 y^2)。在代码里可以这样构造r sqrt(x^2 y^2); H [ x/r, y/r, 0, 0; -y/(r^2), x/(r^2), 0, 0];注意第二行是对方位角的偏导量纲是 1/米反映的是“目标横向移动一个单位方位角变化多少”。这个矩阵如果数值上有数量级差距很大的元素EKF 可能在更新时出现数值不稳定尤其当目标几乎在雷达正前方时方位角对 y 的敏感度会变得特别大。4.3 距离、方位角的噪声协方差 R雷达量测噪声通常是在极坐标空间给出的距离噪声 σ_r角度噪声 σ_θ。滤波器的 R 矩阵就是 diag(σ_r^2, σ_θ^2)。一个常见问题是用直角坐标系下的噪声想想替代极坐标量测的噪声结果模型失真。还有一个更隐蔽的问题是极坐标测量误差虽然在小误差前提下能近似成直角坐标的椭圆分布但目标距离越远同一个角度误差对应的横向位置误差就越大。EKF 的雅可比矩阵天然会把这种距离相关的不确定性通过 H P H R 体现出来所以只要状态和量测的框架正确这个效果是自动获得的。4.4 目标跟踪里最容易出现的“滤波发散”和应对目标跟踪发散有两个典型原因。一是过程噪声给得太小目标一机动新息长期超限但滤波器置信度又很高增益被压得极低状态更新几乎停滞最后轨迹脱靶。解决办法是适当增大 Q或使用自适应方法比如根据新息实际统计值动态调整 Q。二是量测异常值没有预处理。雷达会偶尔冒出野值这些值进入滤波器后会让状态瞬间跳出一个很大的幅度协方差虽然之后会压缩回去但已经污染了航迹。工程上普遍做法是做一个“新息卡方检验”计算 d innov / S * innov如果 d 超过某个阈值比如对应95%置信度的卡方分布临界值就认为这个量测不合格跳过本次更新只做预报。d innov * inv(S) * innov; if d chi2inv(0.95, length(innov)) % 正常更新 else % 野值仅保留预测结果 end5. 地形参考导航高程匹配与非线性地形模型的滤波策略5.1 基本原理两个高度表之差就是一次“地形量测”地形参考导航的思路可以概括成一句话飞行器又装气压高度表又装无线电高度表。气压高度得到的是相对海拔高度的估计无线电高度得到的是相对地面高度的估计。二者相减就在理论上得到了当前所在地面点的海拔高程。问题是这个“测得的高程”无法直接定位因为它没有参考点。但飞行器又带有一块数字高程数据库只要把某个候选水平位置 (x, y) 输入 DEM就能得到“如果我真在这里理论高程应该是多少”。将理论高程与实际测量高程做差残差越小说明候选点越可能是真实位置。这个过程本质上是把位置估计变成一个非线性最优匹配问题。5.2 用 EKF 吸收 DEM 提供的“高程伪量测”我可以把状态选为水平位置、速度以及气压高度计误差等。量测方程写为z_pred h_dem(x, y) h_bias其中 h_dem 是从数字高程数据库中读取的插值高程。它对状态 x 和 y 的偏导就是 DEM 在候选点处的局部坡度这一步需要数值差分或者对 DEM 做平滑插值以后再求导。这里有一个重要的实现细节DEM 数据是离散格网直接读取会得到台阶状的高程函数导数在格点时变得不稳定。因此实际代码里通常用双线性插值或双三次插值生成连续高程面再对插值函数求导。如果不做插值而直接查询格点值EKF 的雅可比矩阵会出现频繁突变滤波协方差很难收敛。5.3 平坦地形上的可观测性灾难如果飞行器飞过一片近乎平坦的平原那么任何水平位置候选点给出的大致是相近的高程。量测残差的差异将变得非常小水平位置的观测量几乎为零。这会造成 EKF 的水平位置协方差无法收缩滤波器退化为纯惯性递推模式位置误差随时间缓慢增长。这种情况不是滤波算法错了而是任务本身的观测信息不足。工程上的应对包括充分利用其他时刻/其他地形特征积累信息或者在定位质量评估中标记当前“地形信息度”低切换到其他导航源。5.4 多假设与粒子化思想EKF 在线性化点偏离真实位置较远时DEM 的实际非线性会让滤波修正量走入错误方向甚至收敛到错误的山谷。解决思路之一是“多点假设”在当前位置附近生成一组候选格网点每个格网点的位置计算其高程残差和似然概率保留高概率点作为新的滤波中心。对应 MATLAB 实现的基本结构大致是% 候选点集合在预测位置周围生成 for i 1:length(candidates) h_theory interp2(DEM_x, DEM_y, DEM, x_c(i), y_c(i), cubic); residual h_measured - h_theory; likelihood(i) exp(-0.5 * residual^2 / sigma_h^2); end likelihood likelihood / sum(likelihood); % 用概率对候选点加权得到本次有效修正这种方法在实践里比纯 EKF 稳健得多尤其适合地形起伏大、初始位置误差大的场景。很多地形参考导航论文里的“最大似然法”“相关定位法”本质上都是这个框架。5.5 地形参考导航的初始化问题地形参考导航有一个前提取条件初始水平位置误差必须足够小否则搜索空间太大候选点数量爆炸匹配结果可能出现多解。工程上通常在进入地形参考导航模式之前先用惯性导航系统全球导航卫星系统把位置误差压制到几百米以内再开始地形匹配。我在仿真里习惯分两阶段第一阶段模拟“预定位”只用惯导递推位置配合卫星量测收敛误差第二阶段关掉卫星量测模拟卫星拒止环境只依靠 DEM 高度差维持位置估计。这样设计更容易观察地形参考导航在“无卫星但有地形起伏”条件下的真实定位能力和发散速度。6. MATLAB实现中的参数调整与调试经验6.1 协方差矩阵的初值试凑顺序很多第一次上手的人一上来就去调 Q 和 R其实一个更合理的顺序是固定量测噪声 R 为传感器手册或经验值。用一个简单的直线轨迹跑通滤波确认位置收敛。再逐渐减小 Q观察是否出现“协方差收缩过快、后续新息进不来”的问题。最后在转弯或机动轨迹上验证 Q 是否足够适应动态变化。这个顺序的核心是先把状态估计问题跑通再处理动态调节问题。如果直接从机动轨迹开始调参几个参数会相互纠缠非常难定位问题。6.2 R 和 Q 的量纲必须一致Q 是过程噪声R 是量测噪声它们都会通过状态方程的量纲影响滤波增益。有人调参数时会面对一个很奇怪的现象位置估计还算正常但速度估计噪声很大原因是 Q 矩阵里速度噪声对应项的物理含义没有和 R 对齐。举个简单例子如果量测是位置量测噪声 R 的量纲是 m²那过程噪声 Q 中对应速度的项就应该通过“速度噪声 × 时间步长后变为位置波动”这个关系来设定而不是直接拍一个数字。一个常用经验是先用误差传播模型把 Q 从传感器噪声等级推导出来再在仿真里放大或缩小二到三倍调整收敛速率。6.3 解析雅可比与数值雅可比的选择EKF 实现里雅可比矩阵既可以用手推的解析式也可以用数值差分近似得到。解析雅可比速度快、精度高但每个任务都要重新推导而且推导错了很难查。数值雅可比用(f(xdx)-f(x-dx))/(2*dx)中心差分近似几乎不需要手推公式但步长 dx 如果选得不好结果会有较大误差。我的经验是复杂量测模型尤其是地形参考导航里的 DEM 插值函数先用数值雅可比跑通算法确认滤波不发散以后再用手推的解析雅可比优化速度并对比两者输出。这样既兼顾开发效率也保证最终实现更稳健。6.4 利用新息序列检查滤波器健康度滤波器的“健康感”可以通过新息序列来观察正常情况下的新息应该是一个零均值、方差基本稳定的序列。如果新息一直同号说明状态里有系统偏差或模型误差如果新息随机但方差特别大说明 R 被低估了实际量测并没有滤波器认为的那么可靠。我调试 INS/GNSS 组合时会把新息直接画出来看位置新息里如果出现锯齿形状基本可以断定是时间同步问题。目标跟踪里如果新息始终偏向同一个方向则大概率是目标模型偏离了真实运动模式。地形参考导航里新息相关性最强因为 DEM 高程本身有空间相关性处理时要结合地形尺度考虑。6.5 从仿真数据过渡到实测数据时的三个改动仿真代码里的理想量测噪声常常是高斯白噪声但实测数据里的误差还有偏差、温漂、异常值和时延。从仿真过渡到实测时我一般先做三件事给每个量测通道单独做离线统计分析确认噪声方差和是否存在常值偏差把量测时间戳和惯导递推时间戳做成对齐最后再加野值剔除逻辑。很多人忽略常值偏差。惯导本身如果初始对准不充分姿态里就带一个小常值偏角GNSS 定位在某些遮挡环境下也有系统偏离。这些偏差如果不补偿EKF 会试图用零偏状态去吸收它最终零偏和位置误差被错误耦合误差特性完全失真。写在代码之外如果只记住一句话那就是卡尔曼滤波器本身给不了你模型但它会在你已经具备的模型基础上用最优统计方式组合量测。INS/GNSS 组合是否稳定常见取决于误差状态设计目标跟踪是否精准往往取决于过程噪声模型匹配实际机动地形参考导航能否锁定位置最依赖地形特征和初始化质量。我自己反复折腾下来的体会是先把标准线性卡尔曼在二维目标跟踪里彻底跑明白把预测、更新、新息检验这些基本功练扎实再去上手扩展卡尔曼滤波的雅可比推导最后进入惯性导航系统全球导航卫星系统组合和地形参考导航会顺很多。MATLAB 的价值在于能把调试周期压缩到很短唯一需要的是像做实验一样去假设、验证、修改假设而不是对着公式空转。
平方复杂度无法被常规优化消除｜TaoToken 统一 Key 通道实测)
