简介面向导航系统设计与分析的一份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 章会展开说这个可观测性问题。常见的参数起步值可以这样给参数符号典型值备注IMU 更新频率fs_imu100 Hz捷联解算周期 10 msGPS 更新频率fs_gps10 Hz位置组合通常 100 ms 一次量测陀螺测量白噪声sigma_g0.01 deg/s折算成 0.0001745 rad/s加速度计测量白噪声sigma_a0.05 m/s^2略大于中低端 MEMS 噪声陀螺零偏初值sigma_bg0.1 deg/h转化为 rad/s 后数量级要看清GPS水平位置噪声标准差sigma_gps1.5 m单点定位给 35 m 更稳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; endQd维度要和状态维度对齐。也可以把陀螺零偏和加速度计零偏建模成随机游走那需要在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 输出做差数值上会差 56 个数量级滤波增益计算出错状态很快发散。正确做法是在仿真开始时固定一个参考点把 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(i1) 之间 w (t_gps - t_imu(i)) / (t_imu(i1) - 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);尾巴上再把惯性纯解算和组合解算的误差画在同一张图里你会直观看到位置组合把长周期漂移抑制在了什么量级。做下一个传感器融合项目时直接复用这套滤波诊断代码比重新搭一套逻辑省半天时间。本文还有配套的精品资源点击获取
