间接卡尔曼滤波实现GPS/INS组合导航的MATLAB仿真全解析

1. 组合导航为什么会选间接卡尔曼滤波先说清楚它解决什么问题做无人机MAV导航的人应该都有这种体会纯靠GPS城市峡谷里飞个几分钟位置跳变能把人整懵纯靠IMU加速度计积分出来的位置误差是随时间三次方增长的几十秒就飘出天际。两个传感器都有硬伤但把它们放在一起用就出现了一个新的麻烦——怎么把两套数据“捏”在一起。最直接的想法是直接卡尔曼滤波把姿态、速度、位置、陀螺仪零偏、加速度计零偏全部塞进状态向量让滤波器直接估计这些物理量。听起来很自然但在工程上坑特别多状态量是姿态角时姿态运动学方程是强非线性的而且欧拉角在俯仰接近90度时会有万向锁问题滤波器直接发散。就算用四元数四元数的归一化约束怎么在卡尔曼框架里严格保持也是个大麻烦。所以工程上更主流的做法是间接卡尔曼滤波也常叫误差状态卡尔曼滤波。核心思路是主滤波器不直接估计姿态、位置这些量而是估计“真实值与标称值之间的误差”。标称值由IMU积分直接给出来这条路很自然因为IMU输出的是角速度和加速度本来就是用来积分的。卡尔曼滤波器只负责估计导航误差比如姿态误差、速度误差、位置误差以及IMU零偏误差。最后用误差去修正积分结果。这个思路最漂亮的地方在于误差状态的运动学方程比原始状态的方程线性得多。小角度近似下姿态误差的动态方程是线性的速度、位置误差方程也能很好地线性化。这样卡尔曼滤波的线性高斯假设基本能满足滤波器稳定性大幅提升。而且GPS的更新频率低5-20HzIMU的采样频率高100-1000Hz间接法天然支持这种异步融合结构高频做IMU积分机械编排低频做误差修正卡尔曼更新。另外还有一个工程上的大便宜不太需要为每个传感器重新设计整套滤波器模型。你想加一个气压计辅助高度只需要把高度误差加到观测方程里想加磁力计辅助航向也是加一个航向误差观测。反正滤波器的状态量是“误差”不是原始物理量外部测量只要能和某个误差状态挂上钩就能非常干净地融进去。这套架构对做飞行器导航、无人车、机器人定位的人特别实用尤其是打算自己写代码而不是只调现成库的朋友。MATLAB里做这个仿真有一个不可替代的好处GPS数据和IMU数据的真实值你说了算你想让GPS在哪个时间段跳变、想给IMU加多大零偏、想模拟多大噪声全部可以精准备注入。这在真实硬件上很难做——因为真实系统里你不知道理想真值也就很难精确评估你的滤波器到底表现好不好。2. 整个仿真的骨架IMU GPS数据是怎么“造”出来的2.1 先定一条真实轨迹而不是上来就调滤波器很多初学者拿到这个项目上来就写卡尔曼滤波的代码结果跑到一半发现滤波器发散又不知道是数据的问题还是代码的问题。我强烈建议第一步先定理想轨迹然后在理想轨迹上叠加传感器误差这样才能在最后对照滤波结果和真值定量评估误差收敛情况。轨迹设计可以直接用MATLAB脚本生成。举例来说设一条包含匀速、转弯、爬升、减速四个阶段的8字形轨迹匀速阶段速度保持15 m/s持续20秒转弯阶段以2度/s的偏航角速度转180度同时高度缓慢上升爬升阶段保持当前航向爬升角8度持续15秒减速阶段速度从15 m/s匀减到8 m/s同时开始左转进入航向稳定段。这条轨迹要提供什么时间、真值位置NED坐标系即北东地、真值速度、真值姿态用欧拉角或四元数。用代码生成后存成数组后续所有IMU仿真和GPS仿真都基于它展开。这里有个容易忽略的点轨迹的加加速度jerk不要太大。因为IMU仿真数据是用运动学方程反推的加加速度太大会导致角速度和加速度跳变剧烈离散化时产生明显误差滤波器再强也会被带偏。一般建议至少以200Hz的时间步长来生成IMU数据如果计算资源允许直接上500Hz更好。2.2 IMU仿真数据的生成细节不只是加个白噪声IMU的真实输出是比力specific force和角速度。在机身坐标系里这两个量可以从导航系下的位置导数和姿态导出[ \omega_{imu} \omega_{nb}^{b} b_g n_g ][ f_{imu} C_{b}^{n} (\dot{v}{n} - g{n}) b_a n_a ]解释一下这个比力公式IMU测量的是比力是加速度计感受到的“推动其质量块运动的力”这个力和载体本身的加速度有关但还要扣除重力影响。所以从理想轨迹反推时先算导航系下的加速度真值然后减去重力矢量再旋转到机体系才能作为加速度计的理想输出。实际仿真中我们需要加三样东西误差项典型数值说明常值零偏陀螺 0.02 deg/s加速度计 0.5 mg表示器件零偏会在积分中持续累积误差高斯白噪声陀螺 0.005 deg/s/√Hz加速度计 0.02 mg/√Hz模拟器件输出上的随机噪声随机游走可加可不加加上更真实零偏本身会缓慢漂移很多网上代码只加白噪声忽略了常值零偏这就漏掉了整个间接卡尔曼滤波最重要的一项——IMU零偏的估计能力。实际上间接卡尔曼滤波里的状态向量必须包含陀螺零偏误差和加速度计零偏误差否则长航时下滤波器会因为有偏补偿不干净而逐渐发散。数据频率方面我一般设置IMU为200Hz。太低会导致积分误差大但太高MATLAB循环会跑得很慢尤其后面还要跑滤波嵌套循环多了速度很影响调试体验。200Hz作为仿真和算法验证的折中点是比较稳的。提示IMU数据在MATLAB里生成必须用循环逐帧算常见错误是用向量化一次算完。不是不能算而是后面卡尔曼滤波本身是一个递推过程你迟早要把IMU数据拆成一帧一帧递进去的。一开始就用循环结构写后面改起来省事。2.3 GPS仿真数据不只是加高斯噪声还要加“跳变”GPS输出的通常是经纬高、地面速度、航迹角。在仿真里我们先用NED位置真值转成经纬高然后在位置和速度上加噪声再转回NED坐标供观测方程使用。GPS噪声模型一般设定为位置噪声水平1米1σ垂直1.5米1σ速度噪声0.1 m/s1σ更新率10Hz光有高斯噪声还不够做这个仿真最有价值的部分其实是“GPS异常”注入。真实GPS在城市环境下会有多路径效应、信号失锁、周跳表现出来就是位置突然跳了几米甚至几十米。仿真里可以这样造数据在80到85秒之间GPS位置加一个15米的水平阶跃偏置在120到122秒之间GPS输出完全丢帧模拟信号失锁在180秒后GPS噪声水平从1米增大到5米模拟进入信号遮挡较重的环境。这几种异常场景注入会让你的滤波器调试工作真正进入“实战状态”而不是只在完美数据上好看。2.4 坐标系的打理这个环节出错滤波必炸做组合导航的人一半时间都在跟坐标系作战。仿真中要用到的坐标系包括惯性系i系GPS和IMU都在地球上工作严格来说要考虑地球自转和科里奥利力。地球系e系ECEF坐标GPS原始输出的位置一般在这个系。导航系n系NED北东地控制回路习惯在这下面工作。机体系b系IMU直接测量的坐标系原点在IMU中心坐标轴沿载体前后上下方向。在保证仿真精度的前提下绝大多数教学级和工程预研级的MATLAB仿真都不会把地球自转项做进去而是直接把NED坐标当作惯性坐标用。这个近似对中低速、短时、小范围的无人机导航完全适用对跨纬度长途飞行不适用。做仿真之前先在代码头部写清楚这个近似条件免得后面跑出奇怪的漂移还在滤波器里找bug。IMU数据必须从导航系转换到机体系。反过来GPS观测是导航系下的量而滤波器输出的误差状态也在导航系两边要统一。最常见的一个错误是姿态旋转方向搞反把 (C_b^n) 和 (C_n^b) 混用导致速度和位置误差看起来在滤波实际上方向全错了。3. 误差状态卡尔曼滤波的原理MATLAB代码要怎么写才能稳3.1 状态向量和系统模型别把维度上错了间接卡尔曼滤波的状态向量选15维或者叫15态姿态误差3维(\delta \psi)速度误差3维(\delta v_n)位置误差3维(\delta p_n)陀螺零偏误差3维(\delta b_g)加速度计零偏误差3维(\delta b_a)这15个状态全部是“误差”不是真实值本身。系统方程写出来长这样[ \delta \dot{p} \delta v ][ \delta \dot{v} -(C_b^n \hat{f}_{imu}^b)^{\wedge} \delta \psi C_b^n \delta b_a v_a ][ \delta \dot{\psi} -C_b^n \delta b_g - v_g ][ \delta \dot{b}g v{bg} ][ \delta \dot{b}a v{ba} ]展开讲一下速度误差方程中那个叉乘项的意义IMU测出的比力是包含重力补偿需求的原始信息姿态误差会导致我们把比力错误地投影到导航系。如果姿态偏了1度水平方向的重力投影误差就会造成一个虚假的加速度速度误差随之累积。滤波器必须能观测到这个耦合关系这正是姿态误差和速度误差绑在一起的原因。状态转移矩阵 (F) 是时变的每一帧都要根据当前的姿态矩阵重新计算。离散化可以采用一阶近似 (\Phi I F \cdot dt)在200Hz的更新频率下这个精度足够。但如果时间步长超过10毫秒建议用matrix exponential函数来计算MATLAB里直接调用这个函数就行性能差异在仿真中不敏感。3.2 机械编排和误差修正一伙的别分开写整个递推过程分成两半机械编排mechanization把IMU原始数据积分得到姿态、速度、位置的标称值。误差修正卡尔曼滤波器估计出误差状态后回代去修正标称值。机械编排的流程是读一帧陀螺和加速度计数据用陀螺数据更新姿态。四元数更新公式为 (q_{k1} q_k \otimes \exp(\omega \cdot dt/2))用姿态矩阵把加速度计测得的比力转到导航系减去重力得到导航系下的加速度积分得到速度和位置回到第1步处理下一帧。很多人在第3步犯迷糊IMU输出的比力是“机体的视加速度”不是“导航加速度”必须减掉重力后在导航系积分。飞行器静止在地面上时加速度计实际输出的是一个g的向上比力如果不减重力积分出来的位置就会带着1g的“加速度”往上飞。误差修正的过程则是卡尔曼滤波器每到一个GPS观测时刻把预测的误差协方差和观测方程一结合算出最优误差估计然后把误差从标称值里减掉再置零。置零这个动作非常重要——误差被修正完以后状态向量应该回落到接近零下一帧的预测才从零附近重新开始。3.3 观测方程GPS的C矩阵是这个滤波器最关键的一行GPS观测设计为位置和速度[ z \begin{bmatrix} p_{gps} \ v_{gps} \end{bmatrix} ]观测矩阵 (H) 就是[ H \begin{bmatrix} I_{3\times3} 0 0 0 0 \ 0 I_{3\times3} 0 0 0 \end{bmatrix} ]简单到离谱。这正是间接卡尔曼滤波的魅力误差状态的观测方程天然线性GPS测的就是位置和速度误差直接取出来就好。如果GPS更新率是10Hz而IMU是200Hz那卡尔曼滤波器的递推是每1/200秒预测一次每1/10秒更新一次。这个时序处理是代码里的核心难点。我的建议是代码里把IMU和GPS数据按时间戳合并排序用一个大循环处理所有帧遇到IMU帧就做机械编排和预测遇到GPS帧就额外做观测更新。千万不要开两个循环分别处理IMU和GPS——这会让时间对不齐滤波器性能大打折扣。3.4 协方差的调参Q阵、R阵不是随便填的Q矩阵描述的是过程噪声R矩阵描述的是测量噪声。这两个矩阵的取值直接决定滤波器的“性格”Q取太小滤波器太“自信”于模型GPS观测更新对状态的修正作用被压制误差收敛慢R取太小滤波器太信任GPSIMU零偏估计不出来因为它的修正全被位置观测抢走了两者比例失调协方差矩阵会出现非正定问题直接发散。我常用的初值是这样[ Q \text{diag}(0.001, 0.001, 0.001, 0.01, 0.01, 0.01, 1e-6, 1e-6, 1e-6, 1e-8, 1e-8, 1e-8, 1e-6, 1e-6, 1e-6) ]含义按状态顺序速度噪声功率 (0.001 (m/s^2)^2/Hz)姿态噪声功率 (0.01 deg^2/Hz)位置噪声功率 (1e-6 m^2/Hz)陀螺零偏随机游走 (1e-8 (deg/s)^2/Hz)加速度计零偏随机游走 (1e-6 (m/s^2)^2/Hz)。R矩阵则直接放GPS的测量方差[ R \text{diag}(1, 1, 2.25, 0.01, 0.01, 0.01) ]对应位置噪声1米水平、1.5米垂直、速度噪声0.1m/s的平方。在实际调参时我一般先固定R调Q看姿态误差的发散趋势然后反过来固定Q调R看位置误差的收敛速度。这个交互过程能用误差曲线直观判断。记住一个原则过程噪声一定要比测量噪声保守宁可让滤波器响应慢一些也不要让它对观测数据过度拟合否则遇到GPS跳变滤波器会跟着跳完全起不到平滑作用。4. 实操全过程从零到能跑出漂亮误差曲线的MATLAB代码4.1 代码结构总览整个项目按模块化拆成以下几个部分每个部分最好单独一个脚本trajectory_generator.m % 生成理想轨迹 imu_simulator.m % 根据轨迹生成IMU数据 gps_simulator.m % 根据轨迹生成GPS数据 ins_mechanization.m % 纯惯导机械编排用于对比误差 error_kalman_filter.m % 误差状态卡尔曼滤波器 run_simulation.m % 主入口脚本 plot_results.m % 绘图对比也可以主入口脚本里用函数调用但核心是一定要分开。因为调试的时候你会频繁想只看轨迹生成、不看滤波结果或者只看位置误差不看姿态误差。每次都要跑完整个run会导致效率极低。4.2 主循环的伪代码这是最容易写乱的地方% 主循环 for k 1:length(imu_time) % 1. 机械编排用IMU数据推进标称值 [q_nb, v_n, p_n] ins_mechanization_step(...); % 2. 计算状态转移矩阵基于当前姿态 F compute_state_transition(q_nb, ...); Phi eye(15) F * dt_imu; % 3. 预测 x_pred Phi * x_err; P_pred Phi * P * Phi Q; % 4. 如果有GPS观测 if gps_time(k) 存在 % 更新 K P_pred * H / (H * P_pred * H R); x_update K * (z_gps - H * x_pred); P (I - K * H) * P_pred; % 5. 误差修正 [q_nb, v_n, p_n] correct_navigation(x_update, q_nb, v_n, p_n); % 6. 清零误差状态 x_err zeros(15,1); else x_err x_pred; P P_pred; end end第4、5、6步的顺序有一个隐藏陷阱GPS观测模型里 (z_{gps}) 应该用GPS原始量减掉“惯导标称值的等效量”还是直接用严格来说观测输入应该是量测值与标称值的差即 (z z_{gps} - h(\hat{x}{nominal}))。但因为H是简单的单位阵加零矩阵所以这里的(z - Hx_pred)实际上等于 (z{gps} - 标称位置/速度)。在代码里写清楚这一步能避免后面加新的传感器观测时出现类型混淆。4.3 数据转换函数经纬高和NED之间的来回切换仿真中轨迹以NED生成GPS以经纬高输出滤波又以NED为工作坐标系所以至少需要三个函数ned_to_geodetic(p_ned, lat0, lon0, h0) geodetic_to_ned(lat, lon, h, lat0, lon0, h0) compute_h_T(lat0)第一个公式用WGS-84椭球模型子午圈曲率半径和卯酉圈曲率半径在代码里写出来。如果用的是球面近似在北纬45度以内误差可以接受但飞行纬度超过60度就会明显偏离真实值。稳妥起见直接用WGS-84MATLAB映射工具箱里有现成的函数没有工具箱也可以手写几行公式。这些转换看似不起眼实际是很多“看起来没毛病但结果就是不对”的根源。我见过不少人直接在NED坐标系上模拟GPS数据省掉了经纬高转换结果整个链路在代码调试期很顺一换到真实GPS数据立刻崩溃。仿真时期就把转换函数写好后面接真实传感器时是最大的一笔节省。4.4 调通以后先做的三件事代码跑通、曲线出来以后不要急着提交或者发给别人。先做三个验证零噪声测试把IMU和GPS的噪声全部设成零。理想情况下误差应该始终保持在零附近卡尔曼滤波的增益不该引起任何状态抖动。如果零噪声情况下误差还发散说明机械编排或观测方程有bug和滤波器无关。纯惯导对照关掉GPS更新只让机械编排跑。这个时候位置误差应该呈现明显的抛物线甚至三次方发散趋势这是纯惯导的典型特征。这个对照能直观展示“不加GPS修正的惯导有多糟”还能验证IMU误差建模是否正确。GPS跳变测试注入前面说的15米跳变观察滤波器的位置误差响应。好的滤波器应该在一两个步骤内把位置误差拉回但速度不要瞬间跳太大。如果速度误差也跟着跳15米说明Q阵调太激进或者R阵太小。这三个测试通过后这个MATLAB仿真才算真正可以拿来做研究或者工程项目预研的底子。5. 仿真结果怎么看误差曲线才是核心产出物5.1 必须画的四张图仿真做完不要只画一张“轨迹对比图”就完事。轨迹对比图只能说明滤波器没炸不能说明它好。必须画以下四类图位置误差对比图横轴时间纵轴北、东、地三个方向的位置误差。一条线是纯惯导误差快速发散一条线是滤波后的误差应该被拉回零附近。这张图最能说明卡尔曼滤波的价值。速度误差对比图同样三条线。速度误差最能反映滤波器的动态响应特性收敛太快可能有振荡太慢说明对GPS信任不足。姿态误差对比图俯仰、横滚、偏航三个角的误差。偏航角误差的收敛通常比俯仰横滚慢因为GPS对偏航的观测是通过速度和位置的间接耦合实现的。如果偏航误差长时间不收敛检查GPS速度观测是否配置正确。IMU零偏估计图陀螺零偏和加速度计零偏的估计值随时间的变化。零偏估计要有一个从初值通常设为0逐渐收敛到接近真实注入值的过程。如果两个零偏根本不动说明Q矩阵对应的随机游走项太小滤波器根本没把零偏当成可估计状态来用。5.2 误差指标怎么量化除了画图还需要算几个具体数字位置误差的稳态RMS值剔除初始收敛段速度误差的稳态RMS值姿态误差的稳态RMS值首次收敛时间误差进入并保持在阈值以内的时间点GPS跳变恢复时间从跳变开始到位置误差回到1米以内的时间这些指标写进博客或者项目报告里比一句“效果良好”有说服力得多。也是你后面调Q阵、R阵时的量化依据——每次调参都记下RMS值很容易找到参数变化和性能变化的对应关系。6. 常见问题与排查技巧这些坑我全踩过6.1 协方差矩阵发散成NaN现象滤波跑着跑着协方差矩阵对角元素变成NaN。排查方向检查状态转移矩阵维度(\Phi)必须是15x15两个矩阵直接相乘时用点乘弄错了检查初始化P阵初始P不能太小因为初始状态不确定度应该较大。建议设成 (\text{diag}(1e-2, 1e-2, 1e-2, 1, 1, 1, ...))姿态误差初始方差给大点没关系反正GPS会很快压下来。检查R矩阵是否奇异GPS观测里如果某一维方差为0矩阵求逆就会爆炸。6.2 滤波结果比纯惯导还差现象加了GPS修正以后误差反而比纯惯导还大。排查方向坐标系转置错误观测值把北和东方向搞反了GPS的更新方向性错误状态直接被拉偏。时间戳错位GPS观测和IMU帧没有对齐导致滤波器把“过去”的GPS数据当成当前状态直接修正产生系统性延迟。这个在仿真中尤其常见因为自己生成数据时会忽略时间同步真实系统里时间戳都是对齐的。观测方程H写错了行GPS位置更新不应该影响速度误差的如果H矩阵里位置观测那行同时激活了速度误差状态之间会互相干扰。6.3 零偏估计收敛到错误的值现象零偏估计曲线稳定了但稳定值不是真实注入的零偏。原因卡尔曼滤波的零偏可观测性是有条件的。在匀速直线飞行阶段加速度计零偏的垂向分量与重力方向会形成对偶歧义——一个固定的垂向加速度计零偏和一个小的姿态误差会同时解释同一个GPS误差滤波器无法区分。所以请勿期望滤波器在所有飞行条件下都能准确估计出零偏。做分析时要看机动段的估计结果。6.4 计算太慢现象200Hz IMU数据、100秒仿真光跑滤波就花了十几分钟。优化方向预分配矩阵不要频繁resize状态维度是15整体矩阵操作很小瓶颈一般是循环次数太多。如果仿真时长不需要全覆盖可以只取60秒代表性区段代码里避免在循环内调用绘图函数。绘图耗时会严重拖慢循环我一般直接用进度输出每1000帧打印一次时间戳仿真结束后统一画图。7. 从仿真到真实系统的几个关键差异MATLAB仿真跑通之后如果你真的打算把这个算法搬到飞控或者车载系统上去有几个差异必须现在就知道传感器特性仿真里的高斯白噪声是理想化的真实IMU里有温度漂移、振动整流误差、采样时钟抖动这些很难用简单高斯项建模。磁力计、气压计也有各自非常强的环境干扰。仿真可以帮你验证算法结构但参数一定需要实测数据重新标定。时间同步真实系统里IMU和GPS各走各的时钟需要专门的PPS或软同步处理。仿真里时间戳是自己生成的不存在不同步问题但真实系统里这个问题能占到整个项目开发工作量的一半以上。数值精度MATLAB默认double精度很多嵌入式平台用float同样是卡尔曼滤波算法float版本的数值稳定性会差很多。协方差矩阵容易失去对称正定性常需要加一个对称化操作 (P (P P)/2)。如果在MATLAB里就养成每步都对称化的习惯移植的时候会省很多事。故障保护真实GPS会长时间失锁可能几十秒到几分钟。滤波器必须能在失锁期间切回纯惯导模式在信号恢复时平滑重切而不是用一次大增益的更新把状态猛拉回去。这需要在卡尔曼框架外加一个gating逻辑或自适应增益策略仿真中可以先模拟这个逻辑。理解这些差异你就不会拿着仿真结果直接去给真实系统下结论。仿真真正的作用是验证算法结构的合理性、确定参数的量级范围、以及培养对误差传播的直觉。真机验证永远是不可替代的。这个用间接卡尔曼滤波做GPS/INS融合的MATLAB仿真做完以后最大的体验就是“误差状态”这四个字是整个算法的灵魂想通了为什么用误差而不是用真值后面所有推导都会自然展开。初次上手时别急着调参先把每一步的物理含义在纸上写清楚再往下写代码效率会高很多。