简介一套面向天文惯性组合导航技术研究的MATLAB仿真程序适用于航空、航天、航海等领域的科研人员、工程师及相关专业学生。该程序将天文导航与惯性导航相结合通过星敏感器与IMU数据融合实现高精度位置、速度与姿态解算可解决纯惯导长时间漂移、天文导航动态响应不足等问题。压缩包共267个文件以193个.m脚本为主体辅以44个.asv备份、26个.mat数据文件、2个.doc说明文档和1个txt说明整体仅698KB结构紧凑便于学习与调试。资源已有466人学习下载常用于教学演示和算法验证。程序覆盖数据采集处理、系统误差建模、卡尔曼滤波融合、仿真环境设置与结果可视化等模块读者可自主设定初始条件评估不同参数对导航精度的影响也可结合文档梳理组合导航算法的完整流程是理解惯性/天文组合导航原理并开展仿真实验的实用工具。1. 天文惯性组合仿真这套 MATLAB 程序里到底有什么值不值得花时间跑通做组合导航的人大多有过这种经历论文里卡尔曼滤波的公式写得明明白白一到自己写代码就翻车——要么状态维度对不上要么量测矩阵方向搞反跑出来的轨迹经纬度能偏出半个中国。天文惯性组合更是把难度拉满星敏感器输出、IMU 积分、姿态解算、滤波融合每个环节都是独立的技术栈串在一起就成了黑匣子。这套天文惯性组合仿真软件程序 version2.1正是为解决这个问题存在的它不是一个商业化封装好的软件而是一整套 MATLAB 仿真脚本集合覆盖从航迹生成、姿态解算、惯性解算到 9 维卡尔曼滤波融合的完整链路。适合正在做天文/惯性组合导航课题的研究生、需要快速搭建算法验证环境的工程师以及想弄懂惯导星敏融合到底是怎么闭环的初学者。资源里那些看起来杂乱的文件名实际上每一份都对应一个明确的算法节点理清楚之后完全可以拿来做算法验证、结果复现甚至二次开发。2. 从文件到功能先把这套仿真程序的模块边界和算法选型看清楚2.1 文件清单背后的功能映射哪些是主程序哪些是辅助验证脚本拿到压缩包解压之后看到一堆 .asv 和 .m 文件第一反应可能是不知从哪下手。这里先不急着跑代码而是把文件按功能分组搞清楚每份脚本在仿真链路里的位置。根据文件名的命名规律这套程序的核心模块大致可以分成五类航迹与场景生成、纯惯性解算、天文观测模拟、组合滤波解算、结果可视化与论文出图。第一类是航迹生成相关脚本典型的是 hull_xy 系列。这类脚本的作用是生成载体运动轨迹包括平面位置、高度变化、姿态角变化为后续的惯性解算和天文观测提供一个真值输入。第二类是纯惯性解算脚本对应文件中的 U1、unti、以及 ins 开头的多个版本负责实现陀螺仪和加速度计的模拟测量、姿态更新算法、速度积分和位置解算。第三类是天文观测模拟与星敏感器相关脚本ins2cns_for_seek_gfov_zuixin_gai 中的 ins2cns 代表从惯性坐标系到天文坐标系的转换过程seek_gfov 则涉及视场搜索或星图匹配逻辑。第四类是组合滤波核心kalman_9w_100ci_angle_right_jianyan 这份文件是整套程序的重心9w 表示 9 维状态量100ci 代表 100 次迭代或某种重复试验次数angle 表示以角度作为观测量right 可能指正确的或右侧安装方式jianyan 是检验脚本说明这个文件专门用来验证滤波器的融合精度。第五类是结果与论文输出paperLA 这类脚本负责把仿真结果画成符合论文格式的曲线图。还有一批 try1、try2 这样的文件是开发过程中留下的试验脚本你可以把这类文件当作探索过程的记录来读也可以直接忽略。这些 .asv 文件是 MATLAB 的自动保存版本表明源码的编写过程中使用过较新版本的 MATLAB但源码本身的算法逻辑是老派的矩阵实现兼容性通常没有问题。2.2 坐标系与姿态描述理解 ins2cns 之前必须先搞懂四个坐标系翻开任何一份组合导航源码最先要过的坎是坐标系。这套程序里涉及四个基本坐标系搞不清楚它们的关系后面看代码就是看天书。第一个是地心惯性坐标系ECI它的原点在地心轴指向春分点方向是天文观测的基准坐标系星敏感器输出的星光方向向量就是在这个坐标系下描述的。第二个是地球坐标系ECEF随着地球自转而转动用于描述载体相对地球表面的位置经纬度就在这个坐标系下定义。第三个是导航坐标系通常取当地北东地或东北天坐标系惯性解算的速度和位置增量都在这个坐标系内积分也是卡尔曼滤波中状态量定义的坐标系。第四个是载体坐标系固连在飞行器或船舶上IMU 的陀螺仪和加速度计输出都是在这个坐标系下的原始测量值。天文惯性组合的关键桥梁就是 ins2cns 这个转换过程它把惯性导航解算出的姿态矩阵与天文观测得到的姿态信息统一到同一个参考坐标系下。常见做法是用方向余弦矩阵表示载体姿态通过星敏感器观测到的星光矢量与星表预报的星光矢量进行匹配得到载体相对惯性空间的姿态角再通过与惯性导航递推的姿态矩阵作差形成滤波器的观测量。代码里 ins2cns_for_seek_gfov_zuixin_gai 这份脚本的 gfov 字段很可能代表着瞬时视场或搜索视场的含义用于模拟星敏感器在有限视场内搜索和识别星体的过程。2.3 为什么用 9 维卡尔曼滤波状态量与量测量的选取逻辑这套程序的核心滤波算法选用 9 维状态量的卡尔曼滤波器这个维度选取是有讲究的。9 维状态量通常包含三维位置误差纬度、经度、高度、三维速度误差东向、北向、天向和三维姿态误差俯仰、横滚、航向这是经典惯导误差模型中的最小完备集合。为什么不把陀螺漂移和加速度计零偏也放进状态量因为这套程序的应用场景是短时高精度天文惯性组合天文观测可以在线修正姿态误差等效于抑制了陀螺漂移的影响所以状态量中不需要显式估计器件误差。量测方程的设计上kalman_9w_100ci_angle_right_jianyan 这个文件名里的 angle 字段透露了关键信息观测量取的是角度而不是位置或速度。常见做法是利用天文观测得到的航向角与惯性导航递推的航向角之差、天文观测得到的水平姿态角与惯导递推的姿态角之差组成量测矢量。这样做的好处是直接对应天文导航的优势——提供高精度的姿态基准同时避开了位置量测需要复杂星图匹配的问题。% 9维滤波状态定义顺序为纬度误差、经度误差、高度误差、 % 东向速度误差、北向速度误差、天向速度误差、 % 俯仰角误差、横滚角误差、航向角误差 syms dL dLambda dh dVE dVN dVU dPhi dGamma dPsi % 状态转移矩阵F的典型构造方式位置误差对应速度误差的积分关系 F zeros(9,9); F(1,4) 1 / (R_M h); % 纬度误差对东向速度误差的系数R_M为子午圈曲率半径 F(2,5) 1 / ((R_N h) * cos(L)); % 经度误差对北向速度误差的系数R_N为卯酉圈曲率半径 F(3,6) 1; % 高度误差对天向速度误差的直接积分关系 % 姿态误差与速度误差之间的耦合项需要根据具体运动状态填充 % 完整的F矩阵应包含惯导误差传播方程中的各项这段代码说明 F 矩阵的前三行对应位置误差与速度误差之间的积分关系是惯导误差模型中位置误差传播的最基本形式。实际使用时第 7~9 行的姿态误差与速度误差之间的耦合项需要根据当前的加速度和角速度来计算程序中的完整实现会在每个滤波周期基于当前运动状态重新构造 F 矩阵。参数 R_M 和 R_N 分别表示地球子午圈和卯酉圈的曲率半径它们随纬度变化程序中一般会内联一个椭球模型函数来计算。从这个例子能看出滤波器的正确性高度依赖状态转移矩阵的精确建模哪怕一个系数的符号搞反整个滤波结果就会出现明显的常值偏置。3. 把整套仿真跑起来从航迹生成到卡尔曼滤波闭环的完整操作步骤3.1 步骤一航迹生成先造一个能用天文观测修正的机动轨迹跑通组合仿真第一步不是直接跑滤波而是先生成载体运动轨迹。这套程序中 hull_xy 脚本的功能就是干这件事它生成一条带有机动动作的水平面轨迹包含直线段、转弯段和加减速段为惯性解算提供真值参考。使用这类脚本时需要注意生成轨迹的时间步长和总时长会直接影响后续滤波的收敛速度和稳态精度。% 生成一条包含直线与转弯的二维航迹并叠加高度变化 dt 0.1; % 仿真步长单位秒对应IMU输出频率10Hz total_time 600; % 总仿真时长600秒即10分钟 t 0:dt:total_time; n length(t); % 初始化航迹数组 hull_x zeros(1, n); % 东向位置 hull_y zeros(1, n); % 北向位置 hull_h zeros(1, n); % 高度 yaw_angle zeros(1, n); % 航向角 % 第一段直线飞行100秒速度v50m/s航向0度朝东 v 50; yaw 0; for i 2:min(n, 100/dt1) hull_x(i) hull_x(i-1) v*dt*cos(yaw); hull_y(i) hull_y(i-1) v*dt*sin(yaw); yaw_angle(i) yaw; hull_h(i) 1000 0.1*(i*dt); % 高度缓慢爬升 end % 第二段匀速转弯90度角速率约为3度/秒 turn_rate 3*pi/180; % 弧度/秒 turn_start 100/dt 1; turn_end 100/dt 30/dt; % 30秒转弯时间 for i turn_start:turn_end yaw yaw turn_rate*dt; hull_x(i) hull_x(i-1) v*dt*cos(yaw); hull_y(i) hull_y(i-1) v*dt*sin(yaw); yaw_angle(i) yaw; hull_h(i) 1000 0.1*(i*dt); end这段航迹生成代码逻辑清晰但需要注意一个细节转弯段实时更新 yaw 值所以转弯过程中的轨迹是一个曲率连续变化的弧线而不是折线。用这种方式生成轨迹的好处是贴合实际载体运动特性惯性解算时加速度计和陀螺仪的输出可以通过对航迹求差分获得。代码中的 dt 参数对应采样步长实际 IMU 输出频率可能是 100Hz 甚至更高但仿真程序为了控制计算量步长通常设置在 0.01~0.1 秒之间。航迹的总时长建议至少覆盖一个完整的机动动作周期这样才能检验滤波器的动态跟踪能力。3.2 步骤二惯性解算推演用角增量和比力更新姿态与位置有了真值航迹下一步就是模拟惯性解算过程。核心思路是从航迹反推 IMU 的理想输出再加上人为设定的误差然后运行捷联解算算法得到带有漂移误差的惯导结果。这套程序中的 U1、unti 脚本就是在做这件事。% 捷联惯导姿态更新使用四元数乘法实现 % 输入上一时刻四元数q_prev角增量dTheta更新周期dt % 输出当前时刻四元数 q_cur function q_cur attitude_update(q_prev, dTheta, dt) % 角增量模值 norm_theta norm(dTheta); % 等效旋转矢量 if norm_theta 1e-12 rotation_vector dTheta; else % 小角度情况下直接用角增量近似 rotation_vector dTheta; end % 构造更新四元数采用一阶近似 dq [cos(norm_theta/2); (dTheta(1)/norm_theta)*sin(norm_theta/2); (dTheta(2)/norm_theta)*sin(norm_theta/2); (dTheta(3)/norm_theta)*sin(norm_theta/2)]; % 四元数乘法q_cur q_prev ⊗ dq q_cur quat_multiply(q_prev, dq); % 归一化防止累积误差导致四元数失去单位模长 q_cur q_cur / norm(q_cur); end姿态更新这步最容易被忽视的是四元数归一化环节。如果不做归一化长时间积分后四元数的模长会偏离 1导致姿态矩阵正交性丧失后续所有解算结果都会污染。代码中讲过了阈值判断当角增量模值小于 1e-12 时仍用原值近似实际使用中这个阈值并不需要太严格因为陀螺输出角增量在正常运动中远大于这个量级。速度与位置更新部分常见做法是在导航坐标系下进行积分需要考虑哥氏加速度和重力加速度的补偿。这套程序中 related like U1 脚本已经在位移积分时考虑了重力的影响这非常重要。如果直接把加速度计输出的比力积分而不扣除重力分量位置误差会在几秒内就爆掉。% 速度更新在导航坐标系下进行比力积分 function [v_next, pos_next] velocity_position_update(v_curr, pos_curr, f_measured, Cbn, dt, g) % f_measured: 加速度计实测比力载体坐标系 % Cbn: 姿态矩阵从载体坐标系到导航坐标系 % 比力转换到导航坐标系 f_nav Cbn * f_measured; % 扣除重力加速度得到真实加速度 a_nav f_nav - [0; 0; g]; % 梯形积分法更新速度 v_next v_curr a_nav * dt; % 位置积分 pos_next pos_curr v_next * dt; end这段代码的结构非常典型直接对应捷联惯导解算的速度方程和位置方程。梯形积分法在步长较小时精度足够如果步长较大或载体机动剧烈建议升级为四阶龙格库塔法。重力加速度 g 的取值可以根据项目场景调整为海平面重力也可以按纬度和高度代入正常重力模型后者在长航时仿真中精度优势明显。3.3 步骤三组合滤波闭环把天文观测角度差送入 9 维卡尔曼滤波器惯导解算跑起来之后会看到位置误差随时间发散这正是纯惯导的正常表现。此时接入天文观测数据用卡尔曼滤波对误差状态进行估计并反馈校正就构成了完整的组合导航闭环。这套程序中的 kalman_9w_100ci_angle_right_jianyan 脚本完成了这一过程的核心逻辑。% 天文惯性组合卡尔曼滤波单步迭代 % 输入状态估计x_prev, 协方差P_prev, 量测z, 观测矩阵H, % 系统噪声协方差Q, 量测噪声协方差R % 输出更新后的状态估计x_cur, 协方差P_cur function [x_cur, P_cur] kalman_step(x_prev, P_prev, z, H, Phi, Q, R) % 时间更新预测 x_pred Phi * x_prev; % 状态一步预测 P_pred Phi * P_prev * Phi Q; % 协方差一步预测 % 量测更新修正 K P_pred * H / (H * P_pred * H R); % 滤波增益 innovation z - H * x_pred; % 新息向量 x_cur x_pred K * innovation; % 状态估计更新 P_cur (eye(size(P_pred)) - K * H) * P_pred; % 协方差更新 end这段代码看起来似乎很简单真正的难度在于构造 Phi 矩阵、H 矩阵和 z 向量。在 kalman_9w_100ci_angle_right_jianyan 的具体实现中Phi 矩阵来源于惯导误差传播方程需要根据载体当前的运动状态实时计算H 矩阵描述了量测与状态之间的关系这里取的角度量测意味着 H 矩阵的 1~3 列对应姿态误差到角度观测量的映射关系。量测 z 向量的构造方式是取星敏感器输出的姿态角航向角、俯仰角、横滚角与惯导捷联解算出的姿态角之间的差值。由于天文观测姿态角精度远高于惯导短期递推精度这个差值主要反映惯导姿态误差的低频部分。% 量测构造示例角度差值作为观测量 attitude_ins_cns ins_attitude - cns_attitude; % 惯导姿态与天文姿态之差 z attitude_ins_cns; % 3x1向量对应俯仰、横滚、航向误差 % 观测矩阵H姿态误状态到量测的直接映射 H zeros(3, 9); H(1, 7) 1; % 俯仰角误差对应量测第1分量 H(2, 8) 1; % 横滚角误差对应量测第2分量 H(3, 9) 1; % 航向角误差对应量测第3分量观测矩阵 H 这样设计有一个前提假设天文观测得到的姿态角误差远小于惯导姿态误差且不考虑安装偏角。实际工程中星敏感器安装支架会有微小的安装误差角这会直接造成组合导航姿态估计的常值偏置。一套合格的程序应该支持在仿真中注入安装误差角并评估其对定位精度的影响。这套程 version2.1 中 kalman_9w_100ci_angle_right_jianyan 文件名里的 right 字段很可能就是对应于某种安装条件下的验证脚本。3.4 参数调优的边界Q 和 R 矩阵应该怎么设才能不发散滤波器跑不通或者精度不对十有八九是 Q 和 R 矩阵设得不合理。Q 矩阵反映的是系统噪声的强度对应陀螺漂移和加速度计零偏等未建模误差R 矩阵反映的是量测噪声的强度对应星敏感器的姿态测量精度。这两组参数的相对大小决定了滤波增益 K 的权重分配。经验法则是如果滤波结果噪声很大说明 R 设置偏小滤波器过于信任量测、对状态预测关注不够如果滤波结果平滑但存在明显偏差说明 R 设置偏大滤波器过于信任系统模型、量测修正作用不足。这套程序中 100ci 这个参数很可能表示进行了 100 次蒙特卡洛仿真目的是统计不同随机噪声条件下的滤波精度分布用这种方式判断 Q、R 设置的合理性。常见做法是先用量测噪声的标准差初始化 R 矩阵再以陀螺随机游走系数和加速度计噪声密度作为 Q 矩阵对角元素的基准值然后通过 3~5 轮试跑调整量级。在跑这套程序时我建议你直接在 kalman 脚本中找到 Q 和 R 的初始化代码把陀螺噪声参数从 1e-10 量级逐步上调到 1e-9 量级观察滤波发散点在哪里这个操作在调参阶段比什么手段都管用。4. 跑这套程序最容易踩的坑四个真实翻车场景与排查建议4.1 现象一滤波器跑 50 步后状态量爆炸现象滤波开始的前几十步还很正常误差估计值缓慢收敛但跑到 50 步左右状态量突然出现 10 的 8 次方量级的数值仿真结果彻底崩掉。原因状态转移矩阵 Phi 的离散化处理不当。连续系统状态方程里的微分项直接乘 dt 得到的离散化矩阵在高机动段会产生数值不稳定。解决使用矩阵指数函数 expm(Phi_c * dt) 计算离散状态转移矩阵而不是采用一阶近似。从工程角度讲这其实是一个经典问题很多人在连续系统转离散时为了省事直接做近似短时间没问题长时间必然翻车。修改之后滤波器会在整个仿真时段内保持数值稳定。4.2 现象二姿态角融合结果有常值偏置现象滤波收敛后俯仰角和横滚角都有 0.2 度左右的常值偏差航向角偏差更大接近 0.5 度而且通过增加仿真时长也无法消除。原因量测方程中缺少安装误差角的标定项或者把天文观测的姿态参考基准理解错了。星敏感器给出的姿态角是载体相对惯性空间的姿态而惯导解算的姿态角是载体相对导航坐标系的姿态二者之间需要经过地球自转和位置矩阵转换。如果 ins2cns 脚本没有正确执行这一转换就会在姿态量测中引入随时间变化的系统误差滤波结果表现为常值偏置。解决检查星敏感器坐标系与载体坐标系的转换矩阵是否在主对角线上有常数偏置并在仿真中增加一个安装误差角的可配置参数然后跑一组不同安装误差角下的仿真对比确认偏置量与注入误差角的线性关系。4.3 现象三程序运行报错 Undefined function quat_multiply现象执行姿态更新脚本时提示 quat_multiply 未定义程序中断。原因MATLAB 版本差异。部分低版本 MATLAB 没有内置四元数工具箱中的 quat_multiply 函数或者该函数在其他工具箱中当前环境未包含相应工具箱。解决不要依赖工具箱函数自己写一个四元数乘法函数标准公式如下。这类问题在 MATLAB 项目里很常见也是我建议复用这类老代码时先检查依赖函数列表的原因。% 四元数乘法适用于标量在前、矢量在后的四元数表示方式 function q_out quat_multiply(q1, q2) q1_scalar q1(1); q1_vector q1(2:4); q2_scalar q2(1); q2_vector q2(2:4); q_out_scalar q1_scalar*q2_scalar - dot(q1_vector, q2_vector); q_out_vector q1_scalar*q2_vector q2_scalar*q1_vector cross(q1_vector, q2_vector); q_out [q_out_scalar; q_out_vector]; end4.4 现象四轨迹闭合实验精度达不到预期现象仿真结束后把组合导航结果与原始航迹对比发现经度方向最大误差有几百米比论文中宣称的精度差一个数量级。原因惯性解算环节中重力模型简化过度。很多简化程序直接使用常量 g9.8没有按纬度和高度修正在 10 分钟级别的仿真中这种简化会造成明显的速度积分误差而滤波器的修正能力有限。解决改用正常重力模型至少加入纬度相关项把加速度计零偏补偿也纳入初始化流程。常见做法是% 正常重力模型计算给定纬度处的重力加速度值 function g normal_gravity(latitude) % WGS84椭球正常重力模型简化项 g0 9.7803267714; % 赤道海平面重力加速度 e2 0.00669437999014; % WGS84第一偏心率平方 sinL sin(latitude); % 纬度和高度修正项 g g0 * (1 0.0052790414 * sinL^2 - 0.0000232718 * sinL^4); end这个重力修正的量级约在 0.05 m/s^2 左右看似微小但 600 秒仿真中在速度上能累积出 30 m/s 的误差对应位置误差超过 1 万米因此必须在惯性解算环节按纬度和高度修正。5. 进阶验证技巧用结果反推算法正确性的三种低成本手段拿到这套程序后除了跑通主流程强烈建议做一遍算法正确性验证。第一个手段是陀螺零偏注入试验在 IMU 陀螺输出中注入一个已知大小的常值漂移量比如 0.01 度/小时然后运行组合滤波观察滤波器能否估计出一个对应的姿态误差常值。如果注入漂移后滤波结果与无注入结果差异不大说明滤波器模型中对陀螺漂移的建模存在缺陷。第二个手段是轨迹闭合检验设计一条从起点出发、经过若干机动后回到原点的对称航迹如果组合导航解算结果在回到起点时误差没有显著累积表示系统误差被有效控制这比只看误差曲线更直观。第三个手段是蒙特卡洛统计利用其 100 次迭代的机制用不同随机种子跑多次统计最终位置误差的均值和标准差与理论上的滤波精度界限对比。这三个验证手段用一遍比看二十篇相关教程理解都深。从那以后我每次拿到别人的组合导航代码都会强制自己先做一遍陀螺零偏注入试验确认滤波器的误差可观测性是否成立再去谈精度。希望帮到你。本文还有配套的精品资源点击获取