UKF工具箱在惯性天文组合导航中的原理与工程实践

简介：这套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，构造 2n+1 个 sigma 点。第 0 个点取均值本身，第 1 到 n 个点取 xm 加上协方差矩阵平方根的第 i 列乘以缩放系数，第 n+1 到 2n 个点取 xm 减去对应列。缩放系数由三个参数控制：alpha 决定 sigma 点离均值的距离，通常取 1e-3 到 1；kappa 是次级缩放参数，高斯分布下取 3-n 比较合适；beta 用于合并先验分布的高阶信息，高斯场景取 2。lambda 的计算式是 alpha 平方乘以(n+kappa)再减 n。

function [X, Wm, Wc] = ut_sigma_points(x, P, alpha, beta, kappa) % 生成缩放对称采样 sigma 点及对应权值 % x: n×1 状态均值; P: n×n 状态协方差 % X: n×(2n+1) sigma 点矩阵; Wm/Wc: 均值/协方差权值 n = numel(x); lambda = alpha^2 * (n + kappa) - n; % 协方差矩阵平方根，用 chol 分解保证结果对称正定 S = chol((n + lambda) * P, 'lower'); X = zeros(n, 2*n+1); X(:,1) = x; for i = 1:n X(:,i+1) = x + S(:,i); X(:,n+i+1) = x - S(:,i); end Wm = zeros(1, 2*n+1); Wc = zeros(1, 2*n+1); 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 点数量从 2n+1 变成 2(n+q)+1，q 是过程噪声维数。计算量确实涨了，但换来的是滤波稳定性明显提升。我一般会把噪声维数控制在 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。这个方法比看数据手册靠谱得多，因为星敏的噪声往往不是你想象的白噪声，而是带低频漂移的着色噪声，这时候 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-off：R 被截到最小值，滤波器会越来越激进，如果后面出现野值，很容易被拉飞。所以 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 end

phi大于 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 end

chi2_thr按量测维度的卡方分布 99% 分位取，3 维量测约取 11.3，2 维量测约取 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 模拟星敏掉线：验证纯惯导漂移与系统恢复能力

一个经常被忽视的验证是量测长时间掉线时的表现。真实场景里星敏会被遮挡，无人机大机动或云层遮挡都会让量测断掉几秒到几十秒。正确行为是滤波器退化为纯惯导递推，姿态误差随时间缓慢漂移；量测恢复后，滤波器能快速收敛回来。如果恢复瞬间滤波直接发散，说明协方差在掉线期间被过度压缩，恢复后的量测在滤波器看来“太可信”，导致修正量过大。这种场景下需要给恢复帧的量测噪声临时放大，或者做协方差注入处理。把这个测试写进你的验证流程，你会少踩很多真实飞行时的坑。我从那以后每次拿到一个新的导航滤波器，都强制走一遍这个掉线恢复矩阵，再谈精度指标。希望帮到你。

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