GPS/INS位置组合导航原理与Matlab仿真实现详解

简介：面向导航系统设计与分析的一份GPS/INS位置组合仿真Matlab源代码包，旨在帮助高校师生、科研人员与工程师掌握组合导航关键实现方法，重点演示GPS外部观测与INS内部传感器数据经卡尔曼滤波融合的完整过程。组合导航利用GPS长期稳定与INS短期高精度的互补特性，通过数据融合弥补单一系统的定位缺陷，是工程中普遍采用的高精度定位方案。整包共九个文件，以四份PDF原理与结果文档、两份Matlab脚本为核心，辅以程序说明TXT、结果报告DOCX及动力学数据MAT，压缩后仅1.48MB，便于边读文档边对照源码。已有503人学习下载，适合正在学习组合导航原理、Matlab仿真建模及滤波调参的读者。内含惯性导航系统方程模型、卡尔曼滤波背景资料及完整仿真流程，读者可运行测试脚本查看位置轨迹与误差统计，也可修改滤波器参数观察不同条件下的定位效果，从而打通从理论推导到代码实现的链路，为实际系统集成提供可复用的实验基础。

1. GPS_INS位置组合仿真在跑什么：先想清楚“组合”到底组什么

GPS会间断、INS会漂移，两者合起来能互相补短。位置组合仿真是组合导航实现方法里最容易被低估的一档：它不需要GPS原始伪距，只拿GPS解算出的位置和INS推算的位置做差，进卡尔曼滤波器估计误差状态再反馈。很多人一上来就写15维“紧组合”，结果发散到天上去；其实先把GPS_INS位置组合的Matlab源代码跑通，搞清误差状态怎么传播、量测矩阵怎么设，后面改紧组合、改伪距、改多传感器才顺。这篇按我调惯组加卫导位置组合的经验，把原理、代码、参数和发散排查串起来，适合暂时没有实机、想把组合导航实现方法在仿真层面验证清楚的工程师或学生。

2. 组合导航实现方法的核心：误差状态卡尔曼滤波（间接法）原理与状态模型搭建

2.1 为什么不直接把GPS位置当观测量进滤波器：误差状态与全状态的选择

组合导航实现方法有直接法和间接法。直接法把位置、速度、姿态做成滤波器状态，GPS位置直接更新这些状态，模型是强非线性的，四元数、方向余弦矩阵、地球参数混在一起，滤波调参很难有直觉。工程里更普遍用的是间接法：先跑一个捷联解算，得到载体的位置、速度、姿态，滤波器只在旁边估计“主状态解算出来的误差”。

间接法有三个实际好处。第一，误差方程在大部分飞行场景下近似线性，协方差传播拿矩阵指数离散化就够，不依赖采样率特别高。第二，误差状态幅值小，小角度假设成立，可以放心用“姿态误差是三维小量”这种模型。第三，位置组合、速度组合、伪距组合共用同一套误差状态，只是量测矩阵 H 不一样，代码结构好复用。位置组合里的量测是 GPS 位置和 INS 位置之差，对应误差状态中的位置误差子向量，所以 H 矩阵在最简单的情况下是 [I 0 0 0 0] 这种形式。

2.2 15维误差状态与系统矩阵 F 的 Matlab 实现

我做惯组加卫导松耦合仿真时，状态量通常取 15 维：

• dr_n：导航系位置误差，3维，单位米
• dv_n：导航系速度误差，3维，单位米/秒
• dpsi_n：姿态误差角，3维，单位弧度
• dbg：陀螺零偏误差，3维，单位弧度/秒
• dba：加速度计零偏误差，3维，单位米/秒平方

状态顺序固定下来后，连续时间状态矩阵 F 的代码如下：

function F = buildStateMatrix(Cbn, f_n) % Cbn : 载体坐标系到导航系的方向余弦矩阵 % f_n : 导航系下比力向量 (包含加速度计测量减去零偏后的比力) F = zeros(15, 15); F(1:3, 4:6) = eye(3); % d(position) / dt = velocity error F(4:6, 7:9) = -skew(f_n); % velocity error 对姿态误差的敏感项 F(4:6, 13:15) = Cbn; % 加速度计零偏误差进入速度误差 F(7:9, 10:12) = -Cbn; % 陀螺零偏误差进入姿态误差 end function s = skew(v) s = [0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0]; end

这段代码把误差传播模型压缩成了三个关键关系。F(1:3,4:6)=eye(3)是运动学关系：位置误差的微分就是速度误差。-skew(f_n)是速度误差对姿态误差的交叉耦合，比力投影到导航系后，姿态角偏差会引起比力方向偏差，进而变成速度误差。Cbn出现在加速度计和陀螺零偏两项里，因为零偏是载体系下定义的，要转回导航系。这里故意没写地球自转角速度、重力异常和科里奥利项，仿真时长在分钟级、速度不太快时这些项影响很小；如果你做高动态飞行器，需要在F的相应子块补-2*Omega_ie这类项。

离散化一般用expm(F * dt)，15 阶矩阵乘指数在普通电脑上压力不大。注意F要在一个 IMU 周期内计算一次，因为Cbn和f_n是时变的。

2.3 量测方程：GPS位置与INS位置之差怎么建H矩阵

位置组合的观测量是 GPS 位置和 INS 位置之差。在局部导航坐标系下，这个差直接对应状态里的位置误差dr_n，量测矩阵最简单：

% 状态维度 15，量测维度 3 H = zeros(3, 15); H(1:3, 1:3) = eye(3);

代码的含义很清楚：滤波器从“位置误差”这个子状态去解释位置残差，姿态误差、速度误差、零偏都不能直接引起瞬时位置观测残差。这带来一个很重要的调参直觉：位置组合对姿态误差和零偏的估计是“间接”的，靠误差传播矩阵把量测信息耦合过去，不像位置误差本身那样直接可观。后面第 4 章会展开说这个可观测性问题。

常见的参数起步值可以这样给：

H矩阵如果状态在 ECEF 坐标系下定义，还需要用一个正交投影矩阵把位置差转到 ECEF，不能直接写[I zeros]。仿真里为了方便排错，我建议先固定一个原点，把 GPS 经纬高用lla2ned类函数转换成局部坐标，INS 同样输出局部坐标，这样H始终不变，调参阶段省掉一堆坐标系转换噪声。

3. 用Matlab源代码实现位置组合仿真的最小闭环：从轨迹生成到滤波器校正

3.1 闭环仿真框架：先造真值轨迹，再叠加传感器误差

要验证组合滤波，第一步不是写滤波器，而是构造带真值的仿真环境。我的做法是先定义一条可重复的轨迹，再根据轨迹生成 IMU 和 GPS 测量。

% 生成一条 60 秒的水平 S 形轨迹 dt_imu = 0.01; t_imu = 0:dt_imu:60; v_true = 20; % 巡航速度 20 m/s Y = zeros(size(t_imu)); % 记录航向角 for k = 2:length(t_imu) Y(k) = Y(k-1) + deg2rad(10) * dt_imu * sin(k * 0.01); % 缓慢变化航向 end pos_true = zeros(3, length(t_imu)); pos_true(1,:) = cumsum(v_true * cos(Y)) * dt_imu; pos_true(2,:) = cumsum(v_true * sin(Y)) * dt_imu; pos_true(3,:) = 100; % 平飞高度 100 m

这里用cumsum生成位置，避免做完整的四元数积分。S 形机动很重要：它能让航向角持续变化，为滤波器提供姿态-位置可观测性。仿真时把真值位置加上 IMU 噪声和零偏，生成加速度计和陀螺测量；GPS 位置则是在真值位置上叠加一个高斯白噪声序列。

我一般把 IMU 和 GPS 的时间戳分开生成，GPS 从第 1 个 IMU 周期开始，每 10 个周期取一个测量点。这样后面能把时间同步问题单独拧出来看。

3.2 松耦合滤波器的传播与更新Matlab代码

主循环是这段仿真里最值得逐行看的部分。

% 初始化误差状态和协方差 x_err = zeros(15, 1); P = blkdiag(eye(3) * 1e-4, ... % 位置误差方差 10cm^2 eye(3) * 1e-2, ... % 速度误差方差 eye(3) * 1e-5, ... % 姿态误差方差 eye(3) * 1e-8, ... % 陀螺零偏方差 eye(3) * 1e-6); % 加速度计零偏方差 I15 = eye(15); Cbn = eye(3); % 初始姿态矩阵，假设水平 g_n = [0; 0; -9.81]; for k = 1:length(t_imu) % 捷联解算的简化写法：用真值比力积分 % 严格应做角速度积分更新姿态，这里用真值轨迹做示意 if k > 1 f_n = Cbn * imu_accel(:, k); % imu_accel 已含零偏和噪声 vel_n = vel_n + (f_n + g_n) * dt_imu; % 注意导航系下加速度 + 重力 pos_n = pos_n + vel_n * dt_imu; end % 滤波时间传播 F = buildStateMatrix(Cbn, f_n); Phi = expm(F * dt_imu); Qd = computeQd(Cbn, dt_imu, sigma_g, sigma_a); P = Phi * P * Phi' + Qd; % GPS量测更新 if mod(k, 10) == 0 z = gps_pos(:, k/10) - pos_n; % GPS位置参考值 - INS位置推算 H = zeros(3, 15); H(1:3, 1:3) = eye(3); R = diag([1.5^2; 1.5^2; 3^2]); % 水平垂直定位精度不同 K = P * H' / (H * P * H' + R); x_err = x_err + K * (z - H * x_err); % 反馈 pos_n = pos_n + x_err(1:3); vel_n = vel_n + x_err(4:6); Cbn = (eye(3) - skew(x_err(7:9))) * Cbn; bg_est = bg_est + x_err(10:12); ba_est = ba_est + x_err(13:15); % 误差状态归零 x_err(1:15) = 0; P = (I15 - K * H) * P; end end

这段代码有四个容易出错的地方。第一，z必须在反馈之前计算，而且用的是当前的pos_n，如果先用pos_n更新再算残差，等于把滤波器刚修完的误差又当成新量测。第二，K = P * H' / (H * P * H' + R)在 Matlab 里使用右除，本质是求解线性方程组，数值上比直接inv(A)再相乘稳定。第三，姿态反馈用了eye(3) - skew(x_err(7:9))，这是小角度近似，如果姿态误差超过几度，建议用四元数更新再转回姿态矩阵。第四，P要在反馈后再右乘，因为量测更新已经降了一部分不确定性，反馈本身不改变协方差，只把误差状态清零。

计算量测噪声的computeQd可以写成这样：

function Qd = computeQd(Cbn, dt, sig_g, sig_a) % 只考虑测量白噪声驱动速度误差和姿态误差 B = zeros(15, 6); B(4:6, 1:3) = Cbn; % 角速度噪声经姿态矩阵加入速度误差 B(7:9, 4:6) = -Cbn; % 加速度计噪声经姿态矩阵加入姿态误差 Qc = blkdiag(diag(sig_g.^2), diag(sig_a.^2)); Qd = B * Qc * B' * dt; end

Qd维度要和状态维度对齐。也可以把陀螺零偏和加速度计零偏建模成随机游走，那需要在B的(10:12, :)和(13:15, :)里再加驱动项。仿真时长超过几分钟时，零偏随机游走不能省略，否则滤波器会过于相信自己的零偏估计。

3.3 状态反馈重置：误差状态归零的正确姿势

间接法误差状态滤波里，最容易被新手改错的点是“重置误差状态”。我第一次调时直接把x_err清零，结果轨迹明显跳变，但最终误差还是收敛了；后来才意识到，重置不是简单置零，而是必须发生在反馈之后。正确顺序是：量测更新得x_err→ 把x_err加进主状态 → 再清零x_err。如果把清零放在反馈之前，主状态永远得不到修正，滤波器只是在孤零零地更新一个误差估计。

反馈完成后，协方差P保持在更新后的值，不需要重新膨胀。这一点和纯航迹推算不同：反馈代表你已经把误差注入主状态，不等于误差消失，只是换了一个参考点继续估计。对位置组合来说，反馈频率一般是 GPS 更新频率，也就是 10 Hz；在两次 GPS 之间，INS 自主解算误差会重新累积，靠P的时间传播来体现这个过程。

4. 位置组合仿真发散排查：数据、时序、单位、可观测性四个维度

4.1 最先看的不是误差曲线，而是新息和协方差

很多仿真跑出来位置误差快速放大，第一反应是去调R。我的经验是先看新息序列，也就是滤波器的“测量残差”。正常收敛时，新息均值接近零，并且落在 ±2 倍标准差的通道内。如果新息持续偏大或者正负不对称，问题多半不在R，而在量测生成或坐标转换。

innovation = z - H * x_err; S = H * P * H' + R; NIS = innovation' / S * innovation;

NIS 是归一化新息平方，自由度为量测维数。位置组合量测是 3 维，NIS 的理论均值是 3，可用chi2inv(0.95, 3)拿到门限。若 NIS 平均值远大于 3，表示滤波器低估了自身误差，Qd或R至少有一个给小了；若远小于 3，往往是你把噪声方差设得太大，滤波器对量测不敏感。把 NIS 画成时间序列，比盯着位置误差去猜发散原因靠谱得多。

4.2 单位与坐标系的坑：经纬度和米混用

GPS 位置组合仿真里最隐蔽的问题是经纬度和米混用。GPS 原始输出是经纬度，如果不转成米，直接和 INS 输出做差，数值上会差 5～6 个数量级，滤波增益计算出错，状态很快发散。正确做法是在仿真开始时固定一个参考点，把 GPS 经纬高转换为相对参考点的北东地坐标，同时把 INS 输出的速度也投影到同一坐标系。

一个常用的快速转换是：

function [n, e, d] = wgs84_to_ned(lat, lon, alt, lat0, lon0, alt0) % 距离较短时使用球面近似，精度够仿真用 R = 6371000; lat = deg2rad(lat); lat0 = deg2rad(lat0); lon = deg2rad(lon); lon0 = deg2rad(lon0); e = (lon - lon0) * cos(lat0) * R; n = (lat - lat0) * R; d = alt0 - alt; end

如果仿真范围超过几十公里，推荐用 WGS84 椭球模型写一个完整的lla2ned，我在源码里一般直接调地理坐标封装函数，避免每个脚本各写一遍。单位换算检查表里最容易漏的是陀螺零偏：很多 MEMS 数据手册写的是 deg/h，代码里要转成 rad/s，漏一个系数 57.3 就直接改写了滤波器增益。

4.3 可观测性不足：位置误差能观，姿态误差为什么飘

位置组合仿真正常情况下位置误差收敛，但姿态误差和陀螺零偏不一定收敛。原因是位置量测对姿态误差只有弱可观测性。飞机平飞匀速直线时，姿态误差、加速度计零偏和位置误差之间存在耦合冗余，滤波器只能把误差分配到它认为最合理的地方，未必是你希望的地方。只有转弯或加减速出现横向比力变化时，姿态误差才被真正激励出来。

处理办法不是加大Qd，而是修改仿真轨迹，让轨迹包含 S 形转向和俯仰变化。我在最小闭环里加入的sin(k * 0.01)航向激励，就是给可观测性“加营养”。你可以在仿真里做一个对照：一条直线轨迹和一条 S 形轨迹，跑完看姿态误差曲线的方差，后者通常会明显下降。

4.4 时间同步误差：GPS 和 IMU 不对齐导致的高频波动

位置组合仿真里还有一个常见问题：GPS 量测取的是某一时刻的位置，但 INS 解算位置在每一拍都有输出，如果不做时间对齐，量测和预测之间会差一个 IMU 周期，短时间看不出来，但会在误差曲线上留下固定频率的毛刺。解决办法是对 GPS 位置做线性插值，让它对齐到某一个 IMU 时刻。

% 假设 t_gps 落在 t_imu(i) 和 t_imu(i+1) 之间 w = (t_gps - t_imu(i)) / (t_imu(i+1) - t_imu(i)); pos_gps_align = pos_gps * w + pos_gps_prev * (1 - w);

这里w是时间比例系数。四元数插值更适合姿态，但位置是米制量，线性插值足够。调仿真时，我会故意把 GPS 时间加一个 5 ms 偏移，看误差是否出现等间隔小跳变，以此判断时间同步代码有没有写对。

5. 从仿真到工程：把位置组合代码改造成可复用模块的 4 个细节

5.1 用 NIS 做参数自检，而不是盯着轨迹好看

位置组合仿真调到轨迹重合，并不代表参数合理。把第 4 章的 NIS 计算封装成一个诊断函数，每次跑完仿真输出平均值和超门限比例。我在实际项目中把这个阈值作为代码合并门禁：NIS 均值落在自由度附近且超限比例低于 5%，才认为这次参数更新有效。这个习惯能挡掉很多“看着没发散但滤波器实际已经失效”的中间态。

5.2 反馈逻辑单独封装，不要在滤波循环里散开

误差状态反馈涉及位置、速度、姿态、零偏四类状态，把它们拆到主循环里很容易漏项。我在工程代码里会写一个applyFeedback(pos, vel, Cbn, bg, ba, x_err)函数，返回修正后的主状态，并在函数末尾统一调用x_err(:) = 0。这样后续加杆臂、加非线性姿态更新，改动范围都被限制在这个函数里。

5.3 画误差包络时，把协方差对角元加进去

很多人只画真实误差和 3 倍真实误差统计包络，却忘了画滤波器自己的协方差。正确画法是每步取出P中对角线的位置误差部分，开根号再乘以 3，得到每条轴的 3σ 包络。如果真实误差经常飞出包络，说明滤波器过自信；如果包络快速收紧而实际误差不收敛，说明模型少了驱动噪声。

plot(t, 3 * squeeze(P(1,1,:)), 'r--'); % 北向位置误差 3σ plot(t, err_pos(1,:), 'b');

尾巴上再把惯性纯解算和组合解算的误差画在同一张图里，你会直观看到位置组合把长周期漂移抑制在了什么量级。做下一个传感器融合项目时，直接复用这套滤波诊断代码，比重新搭一套逻辑省半天时间。

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