简介一套面向组合导航方向的Matlab仿真工程包聚焦卫星组合导航与捷联惯性导航的算法实现适合导航专业学生、科研人员及车载定位算法开发者。工程以实验3车载惯性里程计GPS组合导航实验为主线涵盖初始对准粗/精对准、捷联惯导解算、GPS与里程计数据融合以及多种坐标转换模块有助于理解SINS/GPS组合导航的整体流程与关键公式。压缩包共16个文件主体为15个Matlab脚本m文件包括主程序、姿态解算、坐标变换等子函数另附1个mat格式实测数据文件用于跑通实验包体总大小约61.52MB下载后可按目录直接调用学习。目前已有249人学习下载对于需要快速上手组合导航仿真、阅读或二次开发惯导/GPS融合代码的读者这套资源能提供完整的实验代码与数据结构能有效缩短算法验证与论文复现周期。1. 卫星组合导航仿真卡住你的往往不是滤波算法拿到一个 integrated navigation 工程包第一反应通常是跑通里面的组合导航仿真脚本看看捷联惯导和卫星观测融合后轨迹漂成什么样。但真正动手你会发现滤波器的卡尔曼增益公式谁都能默写卡住你三天三夜的往往是 IMU 噪声像不像真的、GNSS 观测有没有和时间戳对齐、协方差矩阵有没有发疯。这套方案解决的是从零搭一套可复现的卫星组合导航仿真链路生成轨迹、仿真陀螺和加速度计输出、模拟卫星位置观测、用捷联导航机械编排加扩展卡尔曼滤波做紧耦合或松耦合融合。适合刚接手惯导项目想先把原理跑通再上真机的工程师也适合要给组合导航算法做回归验证的老手——仿真跑通了不代表实车能用但仿真跑不通实车一定翻车。2. 惯性仿真和捷联导航机械编排从一条轨迹到 IMU 测量值2.1 轨迹生成是第一步姿态真值别拍脑袋组合导航仿真和纯滤波算法验证有个本质区别你需要一条带真值的轨迹包括位置、速度、姿态以及由姿态和加速度反推出来的角速度与比力。常见做法是先设计一条分段轨迹用多项式或样条曲线描述位置随时间的变化然后对时间求导得到速度和加速度。姿态真值则需要从加速度和航向反推加速度的反方向给出俯仰和横滚航向角由轨迹走向给出。这段轨迹生成代码控制住了仿真的“天花板”轨迹本身不合理后面所有环节都是空中楼阁。角速度真值可以通过相邻两个时刻的姿态矩阵差除以时间步长得到比力真值等于加速度计感受到的比力即导航系加速度减去重力后在载体系下的投影。import numpy as np from scipy.spatial.transform import Rotation def generate_trajectory(total_time100.0, dt0.01): 生成一条水平面内带爬升的轨迹返回位置、速度、姿态、角速度、比力真值 t np.arange(0, total_time, dt) n len(t) # 位置前段直线加速、中段转弯、末段爬升 pos np.zeros((n, 3)) pos[:, 0] 10.0 * t # 东向匀速 pos[:, 1] 5.0 * np.sin(0.02 * t) # 北向正弦摆动 pos[:, 2] 0.5 * t 0.05 * np.sin(0.1 * t) # 天向缓慢爬升 vel np.gradient(pos, dt, axis0) # 速度 位置差分 acc_nav np.gradient(vel, dt, axis0) # 导航系加速度 # 横滚俯仰由导航系加速度反推偏航来自速度方向 roll np.zeros(n) pitch np.arctan2(-acc_nav[:, 0], 9.81 acc_nav[:, 2]) yaw np.arctan2(vel[:, 1], vel[:, 0]) quat Rotation.from_euler(XYZ, np.stack([roll, pitch, yaw], axis1)).as_quat() # 导航系加速度转到载体系得到比力 R_nb Rotation.from_quat(quat).as_matrix() accel_true np.einsum(nij,nj-ni, R_nb, acc_nav np.array([0, 0, 9.81])) # 角速度由姿态差分得到 gyro_true np.zeros((n, 3)) for i in range(1, n - 1): dR Rotation.from_quat(quat[i 1]).as_matrix() Rotation.from_quat(quat[i]).as_matrix().T gyro_true[i] Rotation.from_matrix(dR).as_rotvec() / dt return {t: t, pos: pos, vel: vel, quat: quat, gyro_true: gyro_true, accel_true: accel_true}这段代码里有一个参数最容易被忽略np.gradient的边界处理。首尾两个点的导数是单侧差分精度比中间点低会导致仿真开头和结尾出现尖峰。如果这条轨迹最终要用来验证滤波器的稳态误差建议丢掉前后约 1% 的数据或者用更高阶的差分格式。2.2 IMU 噪声注入白噪声加零偏只是入门Allan 方差才是真功夫捷联导航的惯性仿真里陀螺和加速度计的输出由三部分组成真值、确定性误差、随机误差。确定性误差包括零偏、刻度因子、安装误差随机误差用白噪声加随机游走近似。很多初学者只加高斯白噪声得出的仿真结果漂亮得不像话——因为真实的 MEMS 和光纤陀螺都有明显的零偏稳定性变化长时间积分后位置漂移会完全暴露出来。def simulate_imu(traj, dt, gyro_bias0.02 * np.pi / 180, # 0.02 deg/s 零偏 accel_bias0.01, # 10 mg 零偏 gyro_arw0.01 * np.pi / 180, # 角度随机游走 0.01 deg/sqrt(s) accel_vrw0.05 / np.sqrt(100), # 速度随机游走 0.05 m/s/sqrt(s) gyro_bias_instability0.005 * np.pi / 180): n len(traj[t]) gyro_meas np.zeros((n, 3)) accel_meas np.zeros((n, 3)) bias_gyro np.zeros(3) bias_accel np.zeros(3) for i in range(n): # 零偏慢漂移用一阶马尔可夫近似 bias_gyro -bias_gyro * dt / 100.0 np.random.normal(0, gyro_bias_instability, 3) * np.sqrt(dt) gyro_meas[i] traj[gyro_true][i] gyro_bias bias_gyro \ np.random.normal(0, gyro_arw, 3) / np.sqrt(dt) accel_meas[i] traj[accel_true][i] accel_bias bias_accel \ np.random.normal(0, accel_vrw, 3) / np.sqrt(dt) return gyro_meas, accel_meas注意噪声项除以np.sqrt(dt)这是个高频采样下最容易翻车的地方。惯性器件的角度随机游走单位是 deg/sqrt(s)功率谱密度恒定采样率越高单次采样的噪声幅值越小但积分后的统计特性保持不变。如果漏了这一步把 100Hz 和 200Hz 的仿真结果对比时你会看到截然不同的漂移速度但说不清是算法问题还是噪声标定问题。2.3 姿态更新不能用欧拉角四元数只是起点捷联导航机械编排中姿态更新是最核心的一步。工程实现上几乎不会直接用欧拉角做积分因为万向节锁和三角函数计算量都不可接受。四元数更新是大多数仿真和产品代码的选择但要写成等效旋转矢量形式而不是简单的四元数乘法加归一化。def attitude_update_quat(q, gyro_meas, dt): 简化版四元数姿态更新等效旋转矢量用零阶近似 omega np.array([ [ 0, -gyro_meas[0], -gyro_meas[1], -gyro_meas[2]], [ gyro_meas[0], 0, gyro_meas[2], -gyro_meas[1]], [ gyro_meas[1], -gyro_meas[2], 0, gyro_meas[0]], [ gyro_meas[2], gyro_meas[1], -gyro_meas[0], 0 ]]) dq (np.eye(4) 0.5 * omega * dt 0.25 * (omega omega) * dt * dt) q return dq / np.linalg.norm(dq)这个实现的注释说得很清楚零阶近似。真实工程里高动态场景必须用多子样算法补偿不可交换误差即陀螺在积分区间内旋转方向变化带来的附加旋转。最常用的是双子样或三子样等效旋转矢量法。如果仿真轨迹里包含快速转弯或高机动零阶近似的姿态误差会以不可预测的方式增长且和滤波器的观测噪声混在一起调节参数时会让人误以为是滤波器没调好。速度更新也同理比力积分前要做哥里奥利力和重力补偿。常见写法是v_nav (accel_meas - 2 * omega_en * v_nav g) * dt其中omega_en是地球自转和导航系相对地球的角速度。在纯运动学仿真时很多工程包把这项设为零因为轨迹短、纬度变化小但实际上忽略后会在 10 分钟以上仿真里产生肉眼可见的东向速度误差。3. 卫星观测仿真与 integrated navigation 松耦合 / 紧耦合的选择3.1 GNSS 位置观测直接加高斯噪声前提是不要忽略时间戳卫星组合导航里最省事的观测模拟是在真值位置上加高斯噪声。松耦合架构下GNSS 接收机输出的是经过内部滤波的经纬高和速度作为组合导航滤波器的量测更新。此时观测模型的量测矩阵就是位置和速度的选择矩阵卡尔曼滤波公式简洁状态可观测性也好。def simulate_gnss(traj, obs_interval1.0, pos_noise1.5, vel_noise0.1): 按间隔采样生成GNSS位置和速度观测 t traj[t] dt t[1] - t[0] step int(obs_interval / dt) idx range(0, len(t), step) z_pos traj[pos][idx] np.random.normal(0, pos_noise, (len(idx), 3)) z_vel traj[vel][idx] np.random.normal(0, vel_noise, (len(idx), 3)) z_time t[idx] return z_time, z_pos, z_vel这段代码模拟的是理想情况下的位置域观测。但实际上 GNSS 输出的经纬度和高程方差差异很大水平位置精度在 1 米左右高程误差经常到 2-3 米。滤波器里如果位置观测噪声矩阵用同一个值东向和天向的可信度被等同对待会导致高程通道的漂移被强行压住反而把水平误差带歪。常见做法是设R_pos diag([1.5**2, 1.5**2, 3.0**2])水平和高程分开标定。3.2 时间对齐高频率惯导预测、低频率卫星修正的节奏错位组合导航的难点不在滤波方程而在时间管理。IMU 通常以 100Hz 到 400Hz 运行GNSS 只有 1Hz 到 20Hz。两次卫星观测之间滤波器要跑几十个惯导外推周期这一个流程就是 integrated navigation 仿真的骨架惯导预测、卫星修正、重参数化、再预测。正确的做法是维护一个缓存队列每当 IMU 新数据到来执行状态预测并存储状态向量的副本当 GNSS 观测到达时找到离它最近的惯导状态作为线性化参考点做量测更新然后继续惯导预测。以下是简化版的融合循环框架def run_fusion(traj, gyro_meas, accel_meas, z_time, z_pos, z_vel, dt_imu, Q, R): n len(traj[t]) x np.zeros(15) # 位置3 速度3 姿态误差3 陀螺零偏3 加计零偏3 P np.eye(15) * 1e-4 pos_est np.zeros((n, 3)) vel_est np.zeros((n, 3)) quat_est np.zeros((n, 4)) quat_est[0] traj[quat][0] k_gnss 0 for i in range(1, n): # 惯导预测姿态更新、速度更新、位置更新 quat_est[i] attitude_update_quat(quat_est[i-1], gyro_meas[i], dt_imu) # ... 速度位置积分 ... # GNSS 观测到来时执行量测更新 if k_gnss len(z_time) and abs((i * dt_imu) - z_time[k_gnss]) dt_imu / 2: z np.concatenate([z_pos[k_gnss], z_vel[k_gnss]]) H np.zeros((6, 15)) H[0:3, 0:3] np.eye(3) # 位置观测 H[3:6, 3:6] np.eye(3) # 速度观测 K P H.T np.linalg.inv(H P H.T R) x K (z - H x) P (np.eye(15) - K H) P k_gnss 1 return pos_est, vel_est, quat_est注意这段代码为了展示结构做了大量简化状态x里的姿态误差如何映射到四元数、位置误差如何在导航系更新都需要在第 4 章的状态转移矩阵里补齐。这里想强调的是时间对齐判断条件abs((i * dt_imu) - z_time[k_gnss]) dt_imu / 2不要用i % step 0这种取模方式因为一旦 GNSS 数据有延迟或丢帧取模就会永久错位。最稳妥的办法是给每帧 IMU 和每帧 GNSS 都打上时间戳按时间顺序从队列里取数。3.3 紧耦合的核心伪距域观测和星历无关的替代方案紧耦合比松耦合多一个维度直接用伪距和伪距率作为观测量而不是 GNSS 解算后的位置速度。紧耦合的好处是颗数不足 4 颗时依然可以给滤波器提供约束。但做仿真时你需要先模拟多颗卫星的星历位置然后在载体真值位置上计算几何距离叠加上电离层、对流层和接收机钟差。完整的紧耦合仿真需要卫星星历的简化模型。一个比较省事的办法是假设卫星位置由开普勒轨道参数生成并用一份固定的星座配置。这个方案的工程量和可复现性不如直接用 Position, Velocity, Timing 层面的量测替换——如果你手里没有现成的星历数据源从松耦合切到紧耦合的成本会远远高于收益。多数导航从业者做算法验证时先跑松耦合确认滤波主循环和协方差管理没有问题再决定要不要上紧耦合。4. 15 维状态 EKF 组合导航系统状态如何转移、噪声矩阵如何配4.1 姿态误差用乘性模型别把欧拉角直接塞进状态向量捷联惯导组合导航的经典状态向量是 15 维位置误差 3 维、速度误差 3 维、姿态误差 3 维平台失准角、陀螺零偏 3 维、加计零偏 3 维。这里的姿态误差要用乘性四元数误差模型即真实姿态 估计姿态乘以一个小角度旋转而不是三个欧拉角误差直接相加。用欧拉角误差建模在大失准角或高动态场景下线性化近似会快速失效。状态方程的核心是建立这些误差量之间的耦合关系位置误差的变化率是速度误差速度误差的变化率是姿态误差引起的比力误差加上重力误差姿态误差的变化率近似等于陀螺零偏。连续时间状态转移矩阵的分块结构如下def build_state_transition(F, x, f_nav, dt): 构造离散化的15x15状态转移矩阵 x: 当前状态向量, f_nav: 导航系比力 # F姿态-速度耦合速度误差 -[f_nav]x * 姿态误差 f_skew np.array([ [0, -f_nav[2], f_nav[1]], [f_nav[2], 0, -f_nav[0]], [-f_nav[1], f_nav[0], 0] ]) # 位置误差 - 速度误差位置误差率 速度误差 F[0:3, 3:6] np.eye(3) * dt # 速度误差 - 姿态误差姿态误差率 -Cbn * 陀螺零偏 F[3:6, 6:9] -f_skew * dt F[3:6, 9:12] -f_skew * np.eye(3) * dt # 零偏经比力耦合到速度 F[6:9, 6:9] -np.eye(3) * dt # 姿态误差自耦合 return np.eye(15) F * dt姿态误差这一行的物理含义值得多说一句姿态误差会通过比力投影到速度误差上而陀螺零偏会直接累积成姿态误差。三个通道相互纠缠滤波器要靠 GNSS 的位置速度观测间接把这些误差估计出来。如果轨迹长时间匀速直线飞行水平位置和速度观测对航向误差的可观测性很弱这就是组合导航里常说的“飞直线航向不可观”问题。4.2 调参的玄学Q 矩阵代表你对 IMU 的信任R 矩阵代表你对卫星的信任噪声矩阵 Q 和 R 的取值直接决定滤波器的收敛速度和稳态精度。很多人只调 RQ 用单位阵结果滤波器发散误以为是代码错了。Q 矩阵的物理意义是 IMU 误差的时间累积速率它由器件指标推算而来不是自由参数。参数符号典型取值来源角度随机游走陀螺白噪声(0.01 deg/sqrt(s))^2器件 Allan 方差零偏不稳定性陀螺慢漂移(0.005 deg/s)^2器件 Allan 方差速度随机游走加计白噪声(0.05 m/s/sqrt(s))^2器件 Allan 方差加计零偏不稳定性加计慢漂移(0.01 m/s^2)^2器件标定残余R 矩阵相对简单位置观测噪声为 1.5 米、速度 0.1 米/秒在当前消费级 GNSS 接收机上是合理的。但注意一个细节GNSS 位置误差在静态和动态场景下有系统性差异。城市峡谷中多径效应会让位置误差呈高斯大尾巴分布仿真中只用高斯噪声无法模拟这种场景。如果需要评估滤波器在恶劣环境中的表现需要在 GNSS 观测中额外注入一段缓慢变化的偏差而不是只靠增加噪声方差。def setup_Q(gyro_arw0.01 * np.pi / 180, gyro_bias_inst0.005 * np.pi / 180, accel_vrw0.05 / np.sqrt(100), accel_bias_inst0.005): 构造15维过程噪声协方差矩阵 Q np.zeros((15, 15)) Q[0:3, 0:3] np.eye(3) * 0.01**2 # 位置噪声(很小由速度积分而来) Q[3:6, 3:6] np.eye(3) * accel_vrw**2 # 速度随机游走 Q[6:9, 6:9] np.eye(3) * gyro_arw**2 # 姿态角度随机游走 Q[9:12, 9:12] np.eye(3) * gyro_bias_inst**2 * 0.01 # 陀螺零偏好慢变 Q[12:15, 12:15] np.eye(3) * accel_bias_inst**2 * 0.01 return QQ 矩阵里的位置噪声设很小是因为位置是通过速度积分间接到达的过大的位置过程噪声会破坏滤波器对位置观测的信任。陀螺和加计零偏的慢变项要设得小相当于给零偏施加了一个随机游走约束防止它们被滤波器随意拉来拉去。4.3 初始对准静基座或动基座协方差不能乱给组合导航仿真的初始状态和协方差也是坑最多的环节。如果是静基座启动加速度计的均值可以估计出初始俯仰和横滚陀螺的均值可以粗对齐航向如果是动基座启动就要靠 GNSS 速度方向给航向一个粗值。协方差矩阵的初始值要反映初始对准的不确定性航向误差开 10 度左右对应协方差开 0.03 rad^2方差位置误差给 10 米速度给 1 米/秒。这里有个反直觉的经验初始协方差给太小滤波器会在前几百毫秒内产生剧烈振荡给太大协方差收敛会慢到让人以为滤波器没工作。正确做法是先开大、观察收敛时间、再逐步收紧。如果仿真目标是验证长期稳定性索性把初始协方差固定在一个不太敏感的值上把精力花在 Q 和 R 上。5. 组合导航仿真常见问题排查惯性器件数据翻车、观测错位、协方差发散5.1 姿态在 10 秒内发散但位置误差看起来还能接受现象滤波后轨迹前 10 秒就和真值拉开明显距离姿态误差曲线几乎单调飙升但位置误差曲线反而像有界振荡。原因姿态更新里丢失了不可交换误差补偿项。仿真轨迹带高速旋转时单子样等效旋转矢量无法捕捉角速度方向变化带来的姿态漂移而位置观测把姿态误差“拉”回了位置制造了有界假象。姿态误差仍然真实存在并会持续污染速度通道。解决把姿态更新从单子样换成双子样至少也要用0.25 * (omega omega) * dt * dt这个二阶项。检查轨迹中最大角速度是否接近采样频率的量级如果最大角速度乘以 dt 大于 0.1 rad说明轨迹和采样率不匹配需要增大 IMU 频率。5.2 滤波器的协方差快速收缩到极小之后对 GNSS 观测“视而不见”现象EKF 跑通后协方差对角线在前 200 步内迅速变小到接近机器精度之后的 GNSS 观测几乎不产生任何状态修正轨迹照旧漂移。原因这是卡尔曼滤波的经典过度自信问题。Q 矩阵的零偏项设得太小导致滤波器认为 IMU 是完美的协方差被观测持续压缩最终滤波增益趋近于零滤波器退化成纯惯导积分。解决把 Q 矩阵里的陀螺和加计零偏项恢复到一个保守的水平或者引入协方差下限保护。工程上有一种稳定技巧叫做“协方差钳位”每隔一段固定时间检查 P 对角线的最小值低于阈值就加成一个小量。这不算严格的数学推导但仿真和实机上都很有效。5.3 新息序列呈正弦振荡环路看起来“呼吸”现象量测新息即观测值减去预测值的序列呈现周期性波动频率和轨迹的转弯频率一致但幅度远大于 R 矩阵对应的标准差。原因时间戳错位。GNSS 观测被提前或延后了多个 IMU 周期滤波器在错误的时间里用位置观测修正了状态导致每次修正都引入一个相位滞后。这个现象在纯仿真里极难发现因为仿真中每个时间戳都是理论上精确的。解决在融合循环里加一个时间一致性检查把每个 GNSS 观测时间戳和最近一次 IMU 预测时间戳的差值打印出来。差值的均值应该接近 GNSS 周期的 1/10 以内。出现系统偏差时不要用取模逻辑改用时间戳队列。5.4 陀螺零偏始终不收敛轨迹末端向一个方向缓慢飘移现象滤波器输出的陀螺零偏估计值一直停留在初值附近没有向真值靠近的趋势位置误差在中段很小但在 60 秒之后线性增长。原因可观测性问题。轨迹在大部分时间内是匀速直线航向角不变陀螺零偏通过姿态误差到速度误差再到位置误差的链路不可观测。仿真到 100 秒时只有开头和结尾的转弯段能激励出零偏信息。解决更改轨迹设计每隔一段时间加入一个 S 形机动。这也是真实工程里组合导航系统在车道保持和高速巡航时“跑偏”的理论根源——不是滤波器写错了而是轨迹不给力。仿真阶段把轨迹设计成包含过弯、加减速、爬升的组合零偏估计算法才有机会被验证。5.5 单位混用角度量纲让协方差矩阵变成天文数字现象协方差矩阵对角线突然出现 1e5 量级的数值或者滤波结果在几步内发散到 NaN。原因陀螺仪数据用了度作为单位而姿态协方差初始化用了弧度或者反过来。单位混入状态方程后状态转移矩阵的量纲完全错乱卡尔曼增益的计算结果失去意义。解决入口处统一量纲。陀螺角速度转换为 rad/sGNSS 位置统一用米。最稳妥的办法是全代码只用一个单位制字段参考点经纬度和高度的换算写一个独立函数不要在调用处手动乘以系数。每次跑完仿真打印第一帧数据的单位量级确认没有出现 57.29 这个让人头大的数字。6. 验证组合导航结果轨迹还原误差、协方差曲线和回放一致性仿真的最后一关不是看轨迹图片贴不贴真值而是用三个可量化指标判断这套组合导航系统能不能投入复用。第一是轨迹还原误差的均方根值和时间分布。不要只给一个总 RMSE要看误差的时序曲线如果误差在前 20 秒大、后 80 秒小说明初始对准和协方差收敛过程正常如果误差单调增长说明滤波器没有真正利用 GNSS 观测问题大概率在量测更新或 R 矩阵。代码上做一个简单的统计即可def evaluate_error(traj, pos_est): pos_err np.linalg.norm(traj[pos] - pos_est, axis1) segment 10 # 按10秒分段统计 nseg len(pos_err) // segment seg_rmse [np.sqrt(np.mean(pos_err[i*segment:(i1)*segment]**2)) for i in range(nseg)] return pos_err, seg_rmse第二是滤波器协方差的一致性检查。真实误差应该大致落在3 * sqrt(P_diag)包络内。如果真实误差经常超出包络说明 Q 设小了或者模型有未建模误差如果误差远小于包络说明 Q 被放宽得太保守滤波器精度没有发挥出来。一次性把所有状态的真实误差和协方差包络画在一张图上你会直观地看到哪个通道“信心不足”或“过度自信”。第三是回放一致性。同一组 IMU 和 GNSS 数据在不同机器或不同随机种子下跑两遍结果差异应该远小于传感器噪声带来的误差。如果两次结果差异明显说明代码里有未消除的随机性比如滤波循环里用了全局随机数而没有固定种子或者状态更新顺序在并行环境下不确定。回放一致性是仿真工程被用于算法回归测试的前提做不到这一条后续优化无从谈起。我自己的习惯是仿真脚本里规定死随机种子并把种子数作为每次实验记录的一部分。真机测试数据回放时把 IMU 原始数据直接灌回同一套仿真代码确认实机轨迹和回放轨迹在厘米级内一致再谈算法改动。导航系统最怕的是算法没问题、验证方法先出了问题。仿真做扎实后面实机联调会省掉一大半不必要的调试时间。希望这些经验和参数设置能帮你在 integrated navigation 仿真上少走一段弯路。本文还有配套的精品资源点击获取