简介这套MATLAB程序包围绕鲁棒无迹卡尔曼滤波UKF/AUKF在惯性天文导航中的应用展开面向研究组合导航、滤波算法及MATLAB实现的工程师与高年级学生。压缩包共55个文件以52个m脚本为主附3个txt说明文档总大小仅36KB便于快速下载、解压和本地运行。代码覆盖EKF、UKF、粒子滤波、无迹变换、数值雅可比计算等核心模块并配有demo演示程序同时还提供多版本卡尔曼更新、马氏距离与KL散度等评估工具可对照理解不同非线性滤波器的推导、实现差异与精度表现。目前已有349人浏览学习使用。通过调试这些脚本可在实际场景中融合IMU量测与天文观测处理噪声、异常数据与模型误差提升导航精度与鲁棒性是深入学习惯性天文导航滤波算法与实践应用的实用工具包。1. 鲁棒惯性天文导航的 UKF 工具箱拿到手先弄清楚它能干什么做惯性/天文组合导航的人最怕的不是算法不收敛而是拿到一个 UKF 工具箱读不懂、改不动、参数不敢动。这份 MATLAB 的 UKF utilities 资源核心就是解决这件事它在惯性天文组合导航场景下把 sigma 点生成、状态扩维、量测更新、AUKF 自适应噪声估计这些环节全部拆成可读可改的 MATLAB 函数而不是一个黑匣子。适合正在做 SINS/CNS 组合导航的研究生、刚入职的导航算法工程师以及想从 EKF 切到 UKF 但不想从头推导 UT 变换的人。它能直接回答你三个问题sigma 点怎么生成、星光矢量量测怎么接、噪声协方差怎么在线调。我把它完整拆了一遍下面按使用顺序讲清楚每一步的原理和坑。2. UKF 核心实现sigma 点生成、权值配置与状态扩维2.1 sigma 点生成策略对称采样与缩放采样的取舍UKF 和 EKF 的本质差别在于EKF 对非线性函数做一阶泰勒展开UKF 则通过确定性采样逼近状态分布。UT 变换的核心是用一组 sigma 点经过非线性传播后加权求均值和协方差理论上精度能到三阶。工具箱里最常见的是缩放对称采样生成公式是对于 n 维状态向量 x均值 xm协方差 P构造 2n1 个 sigma 点。第 0 个点取均值本身第 1 到 n 个点取 xm 加上协方差矩阵平方根的第 i 列乘以缩放系数第 n1 到 2n 个点取 xm 减去对应列。缩放系数由三个参数控制alpha 决定 sigma 点离均值的距离通常取 1e-3 到 1kappa 是次级缩放参数高斯分布下取 3-n 比较合适beta 用于合并先验分布的高阶信息高斯场景取 2。lambda 的计算式是 alpha 平方乘以(nkappa)再减 n。function [X, Wm, Wc] ut_sigma_points(x, P, alpha, beta, kappa) % 生成缩放对称采样 sigma 点及对应权值 % x: n×1 状态均值; P: n×n 状态协方差 % X: n×(2n1) sigma 点矩阵; Wm/Wc: 均值/协方差权值 n numel(x); lambda alpha^2 * (n kappa) - n; % 协方差矩阵平方根用 chol 分解保证结果对称正定 S chol((n lambda) * P, lower); X zeros(n, 2*n1); X(:,1) x; for i 1:n X(:,i1) x S(:,i); X(:,ni1) x - S(:,i); end Wm zeros(1, 2*n1); Wc zeros(1, 2*n1); Wm(1) lambda / (n lambda); Wc(1) Wm(1) (1 - alpha^2 beta); Wm(2:end) 1 / (2*(n lambda)); Wc(2:end) Wm(2:end); end这里我特意用chol而不是sqrtm因为sqrtm对非正定矩阵不报错但会给出复数结果chol遇到非正定会直接抛错能在源头拦住协方差发散的问题。权值里Wc(1)多加了(1 - alpha^2 beta)这一项这是为了在高斯分布假设下修正高阶项误差别漏掉。alpha 不要取 1否则 sigma 点离均值太远量测方程强非线性时容易把协方差撑爆。2.2 状态扩维为什么要把过程噪声扩进状态向量惯性导航的状态方程里陀螺和加速度计的零偏、刻度因子误差往往是慢变量直接塞进状态向量没问题。但 UKF 和 EKF 有个关键差别UKF 的时间更新要把过程噪声也作为随机变量参与 sigma 点传播否则协方差预测会系统性偏小。常见做法是把过程噪声 w 扩维进状态向量状态从 x 变成 [x; w]协方差矩阵变成块对角。% 状态扩维示例15 维导航状态 6 维噪声 n_main 15; n_noise 6; x_aug [x_main; zeros(n_noise,1)]; P_aug blkdiag(P_main, Q); % 用扩维后的状态生成 sigma 点 [X_aug, Wm, Wc] ut_sigma_points(x_aug, P_aug, 1e-3, 2, 0); % 时间更新逐个传播 sigma 点 X_prop zeros(size(X_aug)); for i 1:size(X_aug,2) X_prop(:,i) f_state(X_aug(:,i)); % f_state 是离散化后的状态方程 end % 预测均值与协方差 x_pred X_prop * Wm; P_pred (X_prop - x_pred) * diag(Wc) * (X_prop - x_pred) Q;扩维后 sigma 点数量从 2n1 变成 2(nq)1q 是过程噪声维数。计算量确实涨了但换来的是滤波稳定性明显提升。我一般会把噪声维数控制在 6 以内陀螺三轴加加速度计三轴零偏和刻度因子误差留给状态方程去估计。如果状态维数已经很大比如超过了 20 维扩维后的计算量会比较夸张这时候可以考虑不扩维只把状态 sigma 点传播后加上 Q但要注意 P_pred 里 Q 的叠加位置要和你状态方程里噪声的注入位置一致否则协方差预测依然偏小。2.3 sigma 点数量与计算量的边界工具箱里如果状态是 21 维15 维导航状态加 6 维权值sigma 点就是 43 个每个点都要过一遍姿态更新和速度位置积分单步计算量大约是 EKF 的 5 到 8 倍。这在离线处理惯性天文数据时不是问题但如果要做实时仿真就要留意。我见过有人把 UKF 用在 50 维状态上跑一次仿真要一整天后来拆成两个滤波器姿态位置一组、传感器误差一组用级联结构把状态维数砍到 25 以内计算量立刻降下来。选型上记住一点状态维数超过 30或者量测更新频率超过 50Hz先想想能不能拆滤波器别硬上全状态 UKF。3. 惯性/天文组合的量测更新星光矢量与高度角方位角接入3.1 量测方程为什么用方向余弦而不是欧拉角星敏感器输出的是星体坐标系下的星光矢量也就是单位方向向量在星敏测量坐标系里的投影。这个量测和姿态之间是方向余弦矩阵的关系不是欧拉角关系。如果硬把量测写成欧拉角的函数会遇到两个问题一是欧拉角在奇异姿态附近比如俯仰接近正负 90 度方程退化二是星敏输出本身是矢量形式转成角度反而损失了冗余信息。function z h_star_vector(C_bn, vec_star_n) % 星光矢量量测方程 % C_bn: 3×3 姿态方向余弦矩阵导航系到载体系 % vec_star_n: 3×1 导航系下的星光单位矢量由星历计算 % 返回: 载体系下的预测星光矢量 z C_bn * vec_star_n; end对应的量测残差计算里星敏实测的星光矢量要归一化。这里有个容易翻车的地方星敏输出的是像素坐标转换来的矢量如果标定不理想矢量长度不是严格的 1直接拿去算残差会引入偏差。我的习惯是先对实测矢量做归一化再和预测矢量做叉乘或者直接做差。叉乘残差的优势是天然正交于矢量本身不会把矢量长度误差带进滤波器直接做差则简单直观但要求两边都是单位矢量。3.2 高度角方位角量测另一种常见的接入形式有些天文导航方案不用星敏而是用折射星敏感器或者天文导航计算机输出星光的高度角和方位角。这时候量测方程就变成function z h_angles(C_bn, vec_star_n) % 高度角方位角量测先算载体系星光矢量再转成角度 vec_body C_bn * vec_star_n; % 高度角与载体 z 轴的夹角或相对水平面的仰角 elev asin(vec_body(3)); % 方位角x-y 平面内相对 x 轴的旋转角 azim atan2(vec_body(2), vec_body(1)); z [elev; azim]; end角度量测的好处是观测量直观便于和星历数据对照问题是asin在高度角接近正负 90 度时导数趋于无穷UKF 虽然不用算雅可比矩阵但 sigma 点传播时这个非线性会把协方差拉变形。我一般建议优先用矢量量测除非你的星敏输出协议里只给了角度。另外角度量测的单位统一问题很隐蔽——asin和atan2返回弧度但星敏数据手册里有时给的是度这个换算错一位残差会凭空放大 57 倍滤波直接发散。3.3 量测噪声 R 怎么配单位、量级与实测标定UKF 对量测噪声 R 的敏感度比 EKF 更高因为 R 不仅出现在增益里还直接影响新息协方差 Pzz进而控制渐消因子的计算。R 配小了滤波会出现高频抖动但就是不收敛R 配大了估计结果平滑但精度上不去。量测类型典型 R 初值单位一致性要求星光矢量归一化(0.001)^2 × I3量纲一残差是矢量差高度角(0.005 rad)^2确保角度是弧度方位角(0.005 rad)^2同上位置/速度如有 GNSS 辅助按传感器噪声密度标定与状态单位一致我一般会用一段真实星敏数据离线跑一遍统计残差序列的实际标准差再反推 R。这个方法比看数据手册靠谱得多因为星敏的噪声往往不是你想象的白噪声而是带低频漂移的着色噪声这时候 R 只是一个初始值要靠 AUKF 去在线修正。另外注意载体系下的 R 和导航系下的 R 不是一回事如果你的量测方程里乘了方向余弦矩阵R 也要跟着转否则协方差会在坐标系变换中被扭曲。4. AUKF 自适应与鲁棒化噪声在线估计与野值抑制4.1 Sage-Husa 噪声估计器R 和 Q 的在线递推AUKF 里的 A 指的是 Adaptive常见的实现是 Sage-Husa 方法核心思想是用新息序列的实际统计值去修正理论协方差。已知量测残差 nu z_measured - z_predicted则量测噪声 R 的递推估计是function R_est sage_husa_R(nu, Pzz, R_prev, b, k) % Sage-Husa 量测噪声递推估计 % nu: 当前新息残差; Pzz: 新息协方差理论值 % b: 遗忘因子 0.95~0.99; k: 滤波步数 d (1 - b) / (1 - b^k); % 渐消记忆系数 innovation_cov nu * nu; % 实际新息协方差 R_est (1 - d) * R_prev d * (innovation_cov - Pzz); % 保底处理R 必须保持正定 R_est 0.5 * (R_est R_est); R_est max(R_est, 1e-8 * eye(size(R_est))); end注意d的计算方式它随 k 增大而逐渐趋近1-b意味着越到后期对新息的信任越高、对初始 R 的记忆越淡。这个设计是合理的因为你刚启动滤波器时协方差还没收敛新息里混着初始误差不该用来修正 R。b 取 0.96 到 0.98 比较稳妥取太接近 1 会导致 R 收敛太慢取太小则 R 波动剧烈。innovation_cov - Pzz在理论上可能是负的说明实际噪声比滤波器假设的更小这时候 R_est 可能会掉到保底值以下被max截住。这里有个 trade-offR 被截到最小值滤波器会越来越激进如果后面出现野值很容易被拉飞。所以 Sage-Husa 必须配合野值抑制一起用。4.2 新息协方差匹配与渐消因子滤波发散的最后防线如果 Sage-Husa 估计出来的 R 还是压不住发散另一个常用手段是渐消因子。原理是当滤波器实际新息远超理论协方差时说明模型误差太大或者滤波已经趋于发散此时对预测协方差做强制的放大等价于给历史数据“降权”。function [P_pred, phi] fading_factor(nu, S_theory, P_pred, rho) % 渐消因子修正基于新息协方差匹配 % nu: 新息; S_theory: 新息理论协方差; rho: 渐消系数通常取 0.95 nu_cov nu * nu; % 实际与理论的比值用迹的比值作为标量渐消因子 ratio trace(nu_cov) / trace(S_theory); phi max(1, ratio * rho); if phi 1 P_pred phi * P_pred; % 预测协方差放大 end endphi大于 1 才启用小于等于 1 保持原样。这个机制本质上是给滤波器加了保险丝但它是事后补救不是事前预防。如果每个周期 phi 都徘徊在 2 以上说明你的状态方程或者 Q 设置有问题单纯靠渐消因子压着会把 R 的估计也带乱。实际的工程经验是渐消因子只能短期用长期要靠调 Q 和检查量测方程。注意rho这个系数的含义它让 phi 在临界点附近留有余量避免滤波器在收敛和发散之间来回震荡。4.3 野值抑制马氏距离判别与降权处理惯性天文组合导航里野值主要来自星敏被遮挡、太阳光干扰、星图识别错误。野值进入滤波器如果不处理轻则精度骤降重则协方差崩掉拉不回来。常用做法是马氏距离判别function [is_outlier, gain_scale] reject_outlier(nu, S_theory, chi2_thr) % 马氏距离野值判别 % nu: 新息向量; S_theory: 新息协方差; chi2_thr: 卡方阈值 md2 nu / S_theory * nu; % 马氏距离平方 if md2 chi2_thr is_outlier true; % 降权处理按比例缩减增益而不是完全丢弃 gain_scale chi2_thr / md2; else is_outlier false; gain_scale 1; end endchi2_thr按量测维度的卡方分布 99% 分位取3 维量测约取 11.32 维量测约取 9.2。完全丢弃本帧量测是一种策略但我更推荐降权而不是丢弃——因为星敏野值偶尔会在连续几帧都出现全丢会让滤波器这段时间完全靠纯惯导漂移姿态误差会快速积累。降权引入的增益缩放等效于人为调大了 R代价是这一帧量测的修正作用变小但滤波器仍然在运行。这里提醒一句野值判别门槛不能设得太宽否则真正的野值混进来也不能太窄否则高动态机动下的正常新息会被误杀。我一般会在仿真里先把新息序列的统计分布拉出来看一眼再定阈值。5. 常见问题与排查姿态奇异、滤波发散与协方差病态5.1 四元数归一化丢失导致姿态协方差奇异现象滤波跑一段时间后姿态协方差对角线出现负数或者状态估计的姿态四元数模长明显偏离 1滤波性能断崖式下降。原因四元数在状态更新和量测更新后没有被重新归一化。UKF 的状态均值是加权得到的加权平均四元数并不天然满足单位模长约束误差会逐步累积。这个问题的本质是状态流形约束四元数属于单位球面不是欧几里得空间直接做线性加权是有损的。解决在每次状态预测和更新之后强制对四元数部分做归一化处理。更严谨的做法是采用余误差四元数常称为误差四元数表示把四元数均值约束放到运动学方程之外只对三维误差向量做滤波。从实操角度看先做归一化至少能解决多数发散问题。我见过的最常见错误是只归一化了更新的均值没有对 sigma 点传播后的每个点做归一化导致协方差被污染。5.2 滤波发散P 矩阵非正定或者对角线出现负数现象P 矩阵对角线出现负数或者滤波残差越来越大Kalman 增益出现 NaN。原因大部分情况是数值问题叠加上协方差非对称。sigma 点传播后做加权协方差计算时由于浮点误差累积P_pred 会逐渐失去对称性和正定性。另一个常见原因是过程噪声 Q 的设置太小模型误差没有被充分表示导致滤波器过度自信。解决在每轮更新后强制做矩阵对称化也就是把 P 替换成(P P)/2并用chol检查正定性如果失败则对 P 加一个小的对角扰动。我的经验是这一步非常廉价但是效果立竿见影。另外检查 Q 矩阵的物理单位是否正确角速度噪声的方差的单位是 rad^2/s^2如果从数据手册换算错了量纲P 矩阵一定病态。5.3 野值把滤波拉偏表现为姿态跳变后不再恢复现象星敏偶尔跳一个大误差滤波器输出姿态角瞬间偏移几度后面的量测明明正常但姿态就是不回到真值附近。原因UKF 的增益矩阵是通过协方差计算的一旦野值进入新息状态更新会被大幅改向。卡尔曼滤波的前提假设是量测噪声为高斯白噪声野值明显违背这个假设而 UKF 本身对厚尾分布没有鲁棒性。如果没有做野值判别或者判别阈值太大滤波器的协方差会被野值撑大之后的正常量测修正作用被稀释。解决强制启用第 4.3 节的马氏距离判别并且把阈值定得稍微激进一点。另外我建议对星敏量测做多星一致性检查——工具箱如果在观测数据里提供多颗星的残差可以用最小二乘交叉验证先剔除一致性差的星再做滤波更新。这个前置检查比任何鲁棒滤波都有效。5.4 UKF 权重和为负导致协方差计算失真现象sigma 点协方差权值 Wc 有负值在计算 P_pred 时出现非正定。原因缩放对称采样里第一个点的协方差权重 Wc(1) 可能是负的这是 UT 变换的固有特性不是 bug。负权值本身在高斯假设下可以被接受但如果状态维数小、alpha 又取得不恰当Wc(1) 的绝对值会很大加权协方差计算时浮点抵消严重。解决把 alpha 取小一些比如 1e-3这样 Wc(1) 的绝对值会比较可控。另外检查是否用了diag(Wc)做二次型如果 Wc 里有零或负值diag构造是对角矩阵仍然能算但如果你用了repmat做外积再求和负权值带来的数值风险会放大。5.5 单位不统一弧度与度、角速度与线速度混用现象滤波器完全发散但所有矩阵检查都正常log 里也没有 NaN就是估计结果明显偏离物理常识。原因单位问题在组合导航里是最隐蔽的坑。星敏高度角单位是度状态方程里姿态角增量单位是弧度陀螺输出角速度单位是 deg/h状态方程里用的是 rad/s。一旦在某个换算系数上差了一个数量级整个滤波器就在一个错误的尺度上运行。解决在工具箱里找有没有单位转换常量如果没有自己加一段显式转换并打日志。我自己的习惯是在所有函数入口处统一转成 SI 单位弧度、秒、米任何数据手册上的原始单位都在读取处完成转换不在滤波器内部做换算。这个习惯让我少排查了很多诡异的发散问题。6. 进阶用法把 UKF 封装成可复用类并做 EKF 对照验证6.1 用 MATLAB class 封装 UKF 滤波流程工具箱给的往往是散落的函数文件要落到实际项目里最好封装成一个类这样状态维度、噪声矩阵、量测方程都可以作为属性替换不用每次调用都传一大串参数。封装思路是把 sigma 点生成、时间更新、量测更新分成三个方法状态方程和量测方程用函数句柄注入。classdef UkfFilter handle % 简化的 UKF 滤波器类封装 properties n % 状态维数 alpha % UKF 缩放参数 beta kappa x % 状态均值 P % 状态协方差 Q % 过程噪声 R % 量测噪声 f_state % 状态方程句柄 h_meas % 量测方程句柄 end methods function obj UkfFilter(n, alpha, beta, kappa) obj.n n; obj.alpha alpha; obj.beta beta; obj.kappa kappa; end function predict(obj) [X, Wm, Wc] ut_sigma_points(obj.x, obj.P, obj.alpha, obj.beta, obj.kappa); X_prop zeros(size(X)); for i 1:size(X,2) X_prop(:,i) obj.f_state(X(:,i)); end obj.x X_prop * Wm; obj.P (X_prop - obj.x) * diag(Wc) * (X_prop - obj.x) obj.Q; end function update(obj, z) [X, Wm, Wc] ut_sigma_points(obj.x, obj.P, obj.alpha, obj.beta, obj.kappa); Z_pred zeros(size(z,1), size(X,2)); for i 1:size(X,2) Z_pred(:,i) obj.h_meas(X(:,i)); end z_mean Z_pred * Wm; Pzz (Z_pred - z_mean) * diag(Wc) * (Z_pred - z_mean) obj.R; Pxz (X - obj.x) * diag(Wc) * (Z_pred - z_mean); K Pxz / Pzz; obj.x obj.x K * (z - z_mean); obj.P obj.P - K * Pzz * K; end end end封装之后切换不同量测源只需要换h_meas的函数句柄不用改类内部逻辑。工具箱原始代码如果提供了不同的量测函数全部整理成统一签名的形式就能直接套进这个类框架。6.2 用 EKF 对照验证滤波器实现是否正确UKF 实现是否正确最好的验证方式不是盯着残差看而是与 EKF 做对照。在量测方程接近线性、过程噪声高斯的情况下两者的估计误差应该在统计意义上接近。如果差异超过一个量级基本可以确定某一个实现里有 bug。% 对照实验脚本框架 % 同一份仿真数据真值轨迹 惯性测量 星敏量测 % 分别跑 EKF 和 UKF比较姿态误差序列 ekf_err run_ekf(traj, imu_data, star_meas); ukf_err run_ukf(traj, imu_data, star_meas); % 对比 RMS 与最大误差并画出误差曲线叠加图 fprintf(EKF RMSE: %.4f deg\n, rms(ekf_err)); fprintf(UKF RMSE: %.4f deg\n, rms(ukf_err));对照时注意EKF 的量测方程需要用雅可比矩阵UKF 不需要如果你写的 EKF 雅可比有误对照结果会有差异这时候先排查 EKF 而不是怀疑 UKF。另外在强非线性场景下比如大角度姿态机动UKF 精度应优于 EKF这是正常现象不构成 bug。如果 UKF 在强非线性下反而更差优先检查 sigma 点是否经过了正确的状态传播——特别是四元数部分的传播是否用了运动学方程而不是简单的加法。6.3 模拟星敏掉线验证纯惯导漂移与系统恢复能力一个经常被忽视的验证是量测长时间掉线时的表现。真实场景里星敏会被遮挡无人机大机动或云层遮挡都会让量测断掉几秒到几十秒。正确行为是滤波器退化为纯惯导递推姿态误差随时间缓慢漂移量测恢复后滤波器能快速收敛回来。如果恢复瞬间滤波直接发散说明协方差在掉线期间被过度压缩恢复后的量测在滤波器看来“太可信”导致修正量过大。这种场景下需要给恢复帧的量测噪声临时放大或者做协方差注入处理。把这个测试写进你的验证流程你会少踩很多真实飞行时的坑。我从那以后每次拿到一个新的导航滤波器都强制走一遍这个掉线恢复矩阵再谈精度指标。希望帮到你。本文还有配套的精品资源点击获取