GPS+INS组合导航：基于间接卡尔曼滤波的IMU/GPS融合MATLAB仿真

MAV（微型飞行器）在天上飞，最怕的不是风，而是“不知道自己在哪、姿态稳不住”。GPS能给绝对位置，但更新慢、室内遮挡就断；IMU能高速输出角速度和加速度，但积分误差随着时间涨，几秒钟可能还准，飞久一点位置就开始“飘”。把GPS和IMU放到一个滤波器里做融合，就是常说的GPS+INS组合导航。我最近在一个模拟项目X里做了一整轮基于间接卡尔曼滤波的IMU/GPS融合MATLAB仿真，数据全部由仿真生成，跑通后最大的感受是：这个方案看起来公式多，但只要把误差状态、预测更新、量测更新这三段理顺，代码其实很清爽。这篇就完整复盘一下实现思路、传感器仿真、滤波核心和MATLAB实现，适合正在入门组合导航或想把自己仿真数据跑起来的朋友。

1. 为什么MAV导航必须做GPS+INS融合

1.1 单靠GPS的问题

GPS输出的位置和速度是绝对的、不发散的，这对长期导航非常重要。但它的更新频率通常只有10Hz左右，而MAV这种系统控制频率至少需要50Hz甚至更高，光靠GPS做内环控制会被延迟活活拖死。另一个问题是噪声和遮挡，城市峡谷、树荫、隧道、室内环境下GPS直接丢星或跳变，位置可能瞬间偏出去几十米。对于需要连续稳定姿态和位置的MAV来说，单GPS既不够快，也不够稳。

我刚开始做仿真时图省事，直接用GPS位置做差分得到速度，结果速度噪声大得离谱。后来才意识到，GPS速度最好由接收机直接提供（多普勒测速），或者靠滤波平滑，否则你用位置差分得到速度，等于把厘米级的噪声放大成米级每秒的毛刺。

1.2 单靠IMU的问题

IMU（惯性测量单元）最大的优势是高频和短时精度，输出100Hz甚至400Hz的角速度和加速度，在几十毫秒内推算姿态和位置相当准。但IMU天生是一个积分系统，陀螺仪有零偏，加速度计有零偏，这些误差经过两次积分会变成位置误差的三次方增长趋势，通俗说就是“越飞越飘”。

我见过不少新手调纯惯导，一上来就问为什么位置几分钟就飞掉了，其实纯惯导短时能用，长时必须靠外部量测校正。IMU告诉你“我猜你每秒在走几步”，GPS告诉你“你现在大概在哪个路口”，两个单独用都不完美，但放到一个滤波器里就是经典互补：高频预测由IMU顶住，低频绝对修正由GPS拉住。

1.3 为什么用间接卡尔曼而不是直接滤波

直接卡尔曼滤波是把位置、速度、姿态当作状态直接估计，很多人第一反应是这样做。但姿态是非线性的，直接拿四元数或欧拉角当状态，状态方程就变成非线性，线性化困难，还容易碰到欧拉角奇异点。间接卡尔曼滤波（也叫误差状态卡尔曼滤波，Error-State Kalman Filter）是另一条路：不估计位置姿态本身，而是估计“标称解与真实解之间的误差”。

标称解就是惯性导航内部根据IMU积分出来的位置、速度、姿态，这个量可以用高频IMU一直推进。滤波器只去估计位置误差、速度误差、姿态误差、陀螺零偏误差、加速度计零偏误差这些量，然后把误差反馈回标称解。打个比方，纯惯导是“主航线”，GPS是“路标”，滤波器不重画整条航线，只修正“偏差”。这种做法的好处是误差状态都是小量，运动学方程可以很干净地线性化，姿态误差用微小角度表示也天然适配“小扰动”场景。

2. IMU与GPS仿真数据是怎么一步步造出来的

2.1 先定义轨迹和坐标系

仿真数据不能“凭空捏”，你得先有一条物理上合理的轨迹，再用轨迹反推传感器应当看到什么。我在这个项目里用的导航坐标系是NED（北东地），靠近地面的MAV导航用NED很自然，比经纬度方便。轨迹设计我定的是：先悬停3秒，然后向北匀速爬升，再右转90度，最后改为水平平飞，整个航程大概60秒。

为什么要做转弯和爬升？因为滤波对航向的观测性较弱，转弯能“激励”出航向误差，让GPS位置残差间接把航向拉回来。如果全程在直线平飞，融合结果看着也行，但一停就慢慢偏航，这在后面调试里会踩坑。轨迹生成时我会同时算出每个时刻的理想位置、理想速度、理想欧拉角和四元数，这些“真值”后面就是评估滤波精度的基准。

2.2 IMU仿真数据生成的物理本质

IMU输出不是“运动加速度”，而是比力（specific force）。加速度计测到的是机体相对地面的合外力加速度减去重力加速度后的投影，也就是在机体坐标系下的比力。新手最容易错的地方是直接拿trajectory的加速度当加速度计读数，导致后面滤波里重力方向怎么都对不上。

正确的生成方式是：先由轨迹得到NED系下的速度导数和姿态旋转矩阵，然后算比力：

• 加速度计理想输出：fb = R_b^n^T * (v_dot_n - g_n)
• 陀螺理想输出：wb = 通过姿态差分得到角速度

之后再叠加零偏、比例因子误差和高斯白噪声。仿真里我给的陀螺零偏是5度/小时，换算成弧度制大约是2.42e-5 rad/s，如果不转换单位，滤波器大概率直接发散。加速度计零偏我给了0.1mg，也就是约0.98e-3 m/s²，这类微小偏置在纯惯导里很难察觉，但在融合滤波中必须被估计出来，否则位置误差会长期存在一个固定斜率。

2.3 GPS量测数据与丢星模拟

GPS量测仿真比较直接，在真值位置和速度上叠加高斯白噪声。位置噪声我按3σ=1米来给，速度噪声按3σ=0.2米/秒来给，更新频率设成10Hz。这些值量级上是常见消费级接收机的水准，足够验证滤波效果。

我还在第20到24秒之间强制连续丢掉5帧GPS数据，模拟住宅区或树荫下的信号中断。这时候你可以看到滤波只依赖IMU做纯惯导预测，位置误差会慢慢增大，等GPS恢复后再被拉回。这个实验特别能说明组合导航的价值：不是每个时刻都有GPS，但系统不会立刻“爆掉”，而是优雅地退化与恢复。

2.4 时间同步和采样节奏

IMU仿真频率定为100Hz，GPS为10Hz，两者天然不同步。我在MATLAB里用时间向量标定每一个IMU步长，判断“当前IMU时刻是否该来GPS”。如果GPS的真实时间落在两个IMU步长之间，最简单有效的做法是取最近邻帧，而不是线性插值——因为10Hz的GPS和100Hz的IMU相差不太大，插值反而会平滑掉一部分测量噪声，导致滤波器过于自信。

代码里我用一个逻辑数组hasGps(k)标记每个IMU周期对应时刻是否有GPS量测，只在对应步长执行量测更新。这样主循环依然按100Hz跑，姿态和位置的预测更新没有被低频率打断，量测更新只是“顺带”在特定时刻触发。

3. 间接卡尔曼滤波核心实现：15维误差状态

3.1 15维误差状态向量与状态方程

误差状态向量我选了15维，覆盖了MAV导航最核心的可估计量：位置误差、速度误差、姿态误差、陀螺零偏误差、加速度计零偏误差，每个都是3维。用伪公式写就是：

• δp：位置误差
• δv：速度误差
• δθ：姿态误差（小角度）
• δbg：陀螺零偏误差
• δba：加速度计零偏误差

状态方程是误差传播模型，也就是这些误差在一段时间里怎么演化。速度误差方程里有一个关键交叉项：姿态误差通过旋转矩阵和比力耦合进速度误差，这就是为什么姿态估计会和位置估计强耦合。陀螺零偏会积分进姿态误差，加速度计零偏会积分进速度误差，整个状态空间就是“谁影响谁”的因果链。

MATLAB里我直接用分块矩阵去搭F矩阵，每一块代表一个3×3子矩阵。这样写虽然看起来繁琐，但调试时比写成一个大矩阵要清楚得多。我建议你也用分块写法，哪个块对应什么物理量一目了然。

3.2 预测：从误差方程到协方差递推

误差状态是线性的，离散化很好做。状态转移矩阵用一阶近似：Φ = I + F * dt，在100Hz的步长下这个近似精度完全够用。协方差预测用标准形式：P = Φ * P * Φ' + Q。

这里的Q是过程噪声协方差，描述IMU噪声和零偏随机游走注入到误差状态的不确定性。Q的数值不是随便填的，它要由IMU噪声方差换算到状态空间。比如陀螺白噪声会通过姿态误差方程进入状态，加速度计白噪声会通过速度误差方程进入状态。我在仿真里把Q调得稍微偏大一点，给滤波器留出余量，让量测更新能够较快响应真实误差。

实际调Q时有个经验：Q太小，滤波会过度信任IMU预测，GPS量测更新效果不明显，误差曲线会像“拉过头”；Q太大，融合轨迹会明显偏噪声，甚至把GPS毛刺都吃进去。Q和R是成对调的，改一个不动另一个，很难找到感觉。

3.3 量测更新：GPS进来之后怎么办

GPS量测我采用松耦合方式，观测量是位置和速度，耦合关系简单直接。量测方程为：z = H * x_error + 噪声，其中H矩阵把GPS位置误差对应到状态里的位置误差，GPS速度误差对应到状态里的速度误差。姿态误差和零偏误差在GPS量测里没有直接观测量，它们只能通过运动学耦合被间接估计出来。

更新顺序是这样的：先计算GPS位置速度和当前标称位置的残差，注意残差是“GPS量测 - 标称解”，而不是“真值 - 标称解”。因为卡尔曼滤波本身就是在处理含噪量测，残差里同时包含了真实误差和测量噪声。然后算卡尔曼增益，更新误差状态协方差，得到误差状态估计。

很多教程把这个地方讲得云里雾里，我自己的理解是：量测更新阶段，你只是知道“标称解偏离GPS量测多少”，并不知道系统到底错在哪，是位置错还是零偏错。滤波器通过协方差矩阵在多个状态之间做“归因”，把一部分残差分配到位置误差，一部分分配到速度误差，一部分分配到零偏误差。这个归因过程就是卡尔曼增益的本质。

3.4 闭环修正与误差状态重置

间接卡尔曼滤波有个关键操作：每次量测更新之后，要把估计出来的误差反馈回标称状态，然后把误差状态清零。位置和速度修正很简单，直接把误差加回去。姿态修正要麻烦一点，不能直接把δθ加到四元数上，而是把δθ转成微小的姿态增量四元数，再和当前四元数做一次组合，最后归一化。

零偏反馈也要做：陀螺零偏估计和加速度计零偏估计是在线更新的，每次量测更新后就把这些偏置加到IMU补偿模块里。下一拍IMU读入的是补偿后的测量，这样误差不会越积越大。最后别忘了把误差状态向量置零，因为误差已经“合入”标称解了，如果不置零，下一拍会重复修正同一个误差，滤波器直接变成“双重积分”。

我在调试时对这步印象很深，第一次写漏了重置，结果位置曲线一直在GPS附近反复震荡，看起来像过冲，其实根本不是滤波发散，就是重复修正。

4. 完整MATLAB仿真主流程与参数调优

4.1 主循环代码结构

仿真主循环在MATLAB里大概长这样：

for k = 1:N_imu dt = t_imu(k) - t_imu(max(k-1,1)); % 预测：用IMU补偿后的比力和角速度推进标称状态 [x_nom, P] = predict(x_nom, P, fb_comp, wb_comp, dt); % 误差状态在预测后清零（名义上是0，协方差已传播） x_err = zeros(15,1); % 量测更新：只有GPS帧才执行 if hasGps(k) z = [gps_pos(k,:)'; gps_vel(k,:)'] - ... [x_nom.pos; x_nom.vel]; H = [eye(3), zeros(3), zeros(3), zeros(3), zeros(3); zeros(3), eye(3), zeros(3), zeros(3), zeros(3)]; R = blkdiag(R_pos, R_vel); K = P * H' / (H * P * H' + R); x_err = K * z; P = (eye(15) - K * H) * P; P = 0.5 * (P + P'); % 强制对称 end % 反馈修正标称解 x_nom = correctNominal(x_nom, x_err); end

这个结构里，预测在每个IMU周期都执行，量测更新只在GPS时刻触发。correctNominal函数负责把误差状态合并进位置、速度、四元数和零偏，同时返回置零后的误差状态。整体代码量不大，核心就是这一段。

4.2 关键参数配置表

我把这次仿真用的关键参数整理成一个表，方便你抄作业后自己改：

我特别提醒单位问题，陀螺零偏涉及度、弧度、小时、秒的换算，错一位小数就会让滤波器发散。仿真一开始我先打印各状态误差的绝对值，确认没有指数级增长，再去调Q和R，这种“先看趋势，再看数值”的顺序在调试时非常省时间。

4.3 结果怎么评判

仿真跑完后，把真值轨迹、纯GPS点、纯惯导和融合结果放到一起对比，效果其实非常直观。纯惯导在60秒内位置漂出去几十米，纯GPS看起来是一条“带毛刺的抖动线”，而融合后的轨迹既贴合真值又平滑连续，毛刺明显被滤掉，位置误差基本控制在1米以内，速度误差在0.2米/秒以内。

我看结果不会只盯均值误差，还会看零偏估计的收敛曲线。一个好的滤波会让陀螺零偏估计在20秒左右收敛到设定值附近，加速度计零偏也会慢慢逼近真值。如果零偏一直不收敛，别急着加噪声，先回头检查H矩阵写对没有、GPS残差方向有没有搞反。这些“软件坑”比“公式坑”更容易被忽略。

5. 常见问题与排查技巧实录

5.1 最容易翻车的几个坑

我在这个项目里踩过的坑基本可以列成一张速查表：

这张表看着简单，但每一个都是“看起来一切正常，但结果悄悄烂掉”的典型。我印象最深的是重力补偿，刚跑仿真时位置曲线在天上飞，查了半天才发现是加速度计仿真时没有减去重力，等于把重力当成了持续推力，滤波器再厉害也救不回来。

5.2 一套可复制的排查顺序

遇到结果不对，我建议你按这个顺序排查，不要乱试参数。第一步，把GPS量测噪声方差设成极大值，相当于让量测更新几乎失效，看纯惯导积分是否稳定，如果漂移或跳变，问题出在IMU仿真或标称解更新上。第二步，把量测噪声恢复，Q先给一个偏大的值，看残差是否被拉回来，如果位置能被拉回，说明闭环逻辑基本通。

第三步，加入固定零偏，看滤波器能否在几十秒内估计出零偏。如果估计出来的零偏和设定值符号相反，八成是姿态修正方向写反了，比如把误差姿态加成了“负号”。第四步，再模拟GPS丢星，看恢复后的收敛速度是否合理。整个流程用不了一小时，但能筛掉绝大多数低级错误。

还有一个特别实用的小技巧：在每个GPS更新时刻，打印残差的模长。如果残差突然从0.1米跳到10米，不用怀疑，量测更新代码里肯定有某个维度搞反了，或者是GPS数据和IMU时间戳没有对齐。时间戳错位在仿真里也能模拟出来，我一试就发现影响很隐蔽，但不查的话会一直觉得“滤波器噪声好大”。

6. 从仿真走向真机的一些扩展思考

6.1 真机数据接口的差异

仿真跑到最后，这个滤波算法本身是“传感器无关”的，只要你把IMU和GPS数据换成真机采集的，核心代码基本可以复用。但真机数据有仿真里没有的问题：时间戳不同步、GPS丢包不是连续的、IMU噪声有色而非纯高斯白、传感器标定误差更大。这些会带来新的挑战，但间接卡尔曼滤波的框架本身足够鲁棒，只要做好时间对齐和初始化，并不会因为“数据更脏”就崩掉。

我在仿真里刻意模拟了丢帧和噪声异常，就是要让算法在“不那么完美”的输入条件下也能稳住。真机上还有一个仿真里不会出现的问题：机体振动会直接耦合进IMU数据，所以你需要先做低通滤波或使用更高阶IMU频响模型。这点我建议在仿真阶段就把IMU噪声方差调得比理想值稍大一点，给真机预留余量。

6.2 后续怎么扩展

这个仿真项目再往下走，有几条很自然的扩展路径。第一，加入磁力计模拟，把三轴磁强计量测放进量测更新，专门治航向误差慢收敛的问题。第二，把松耦合升级成紧耦合，直接用GPS伪距做量测，这样在可见卫星少时依然可以用，代价是实现复杂度高一个数量级。第三，把运动模型从简单的惯性模型扩展成带气压计和空速管的模型，对MAV的高度估计会有明显改善。

我个人在实际操作中的体会是，15维间接卡尔曼滤波是组合导航里性价比极高的一块基石，一旦跑通，后面加传感器、换轨迹、改参数都只是在同一个骨架上做文章。最后再分享一个小技巧：别急着用滤波去融合一组你根本没看过真值的仿真数据，先把你生成的IMU数据拿到纯惯导积分里看一眼，确认物理一致了，再进滤波闭环。这一步能帮你把“传感器模型错误”和“滤波算法错误”彻底切割开，剩下的调试才会真正高效。