运用强跟踪无迹卡尔曼滤波来实现捷联惯导的初始对准

运用强跟踪无迹卡尔曼滤波（Strong Tracking Unscented Kalman Filter, STUKF）来实现捷联惯导（SINS）的初始对准

相比于传统的扩展卡尔曼滤波（EKF）和普通的UKF，STUKF 能够克服系统模型不准确或外界存在强烈干扰时的**"滤波器发散"**问题，极大提升了初始对准的速度和精度。

一、为什么要对SINS用"强跟踪UKF"做初始对准？

1. 问题的本质：在大姿态角下，系统是非线性的

捷联惯导的初始对准，本质是估计载体当前的姿态（航向角、俯仰角、横滚角）。当初始姿态误差角（尤其是航向角）较大时，SINS的姿态更新方程呈现出强烈的非线性。普通的线性卡尔曼滤波（LKF）直接失效，EKF虽然能用泰勒展开线性化，但在大角度下误差会急剧累积。

2. 引入 UKF（无迹卡尔曼滤波）

UKF 通过无迹变换（Unscented Transform），用一组确定的"Sigma采样点"来逼近非线性系统的概率分布。它不需要计算繁琐的雅可比矩阵，且对非线性系统的逼近精度能达到三阶以上，完美解决了大失准角的非线性问题。

3. 引入 "强跟踪" 机制（杀手锏）

在实际工程中，车辆或舰船在地面晃动、发动机震动等复杂环境下，系统噪声往往是不确定的。普通UKF遇到这种强干扰容易"跟丢"。强跟踪滤波（STF） 的核心思想是：在传统滤波的增益矩阵中，引入一个渐消因子（Fading Factor, λ\lambdaλ） 。当检测到系统残差突然变大（受到干扰）时， λ\lambdaλ 会自动放大，强行拉回滤波器的注意力，使其具有极强的鲁棒性。

二、 STUKF-SINS 初始对准的数学模型

在工程实现中，我们通常建立大方位失准角误差模型 ，选取 9 维的状态向量 XXX：
X=[ϕEϕNϕUδvEδvNδvUϵxϵyϵz]TX = \begin{bmatrix} \phi_E & \phi_N & \phi_U & \delta v_E & \delta v_N & \delta v_U & \epsilon_x & \epsilon_y & \epsilon_z \end{bmatrix}^TX=[ϕEϕNϕUδvEδvNδvUϵxϵyϵz]T

ϕ\phiϕ：失准角（东、北、天）
δv\delta vδv：速度误差
ϵ\epsilonϵ：陀螺仪随机常值漂移（需要被系统在线标定出来）

观测向量 ZZZ ：
Z=[δvEδvN]TZ = \begin{bmatrix} \delta v_E & \delta v_N \end{bmatrix}^TZ=[δvEδvN]T

我们将捷联解算出的速度与GPS或外部参考速度做差，作为观测输入。

三、 Matlab 实战代码框架与核心算法

实现 STUKF 初始对准的 Matlab 核心骨架。请注意，这是高度提炼的工程级伪代码，直接复刻了工业界的开发思路。

1. 主循环框架

matlab 复制代码

clc; clear; close all;

%% 1. 参数初始化
dt = 0.01;                 % 采样周期 100Hz
T_total = 60;              % 总对准时间 60秒
N = T_total / dt;          % 总步数

% 状态维度 n=9 (失准角3 + 速度误差3 + 陀螺漂移3)
n = 9; 
m = 2;                    % 观测维度 (速度误差Vx, Vy)

% 初始状态估计
X_est = zeros(n, 1);      
P = diag([1, 1, 10, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1].^2); % 初始协方差

% 噪声协方差
Q = diag([1e-5, 1e-5, 1e-5, 1e-4, 1e-4, 1e-4, 1e-6, 1e-6, 1e-6]); % 过程噪声
R = diag([0.1^2, 0.1^2]); % 观测噪声

%% 2. 生成模拟传感器数据（SINS解算的速度和真实参考速度）
% 注：实际工程中此处应读取IMU数据和外部速度计数据
[imu_data, ref_vel] = generate_simulated_data(T_total, dt);

%% 3. STUKF 滤波主循环
for k = 1:N
    % ===== A. 构造 Sigma 采样点 =====
    % Wm: 均值权重, Wc: 协方差权重
    alpha = 1e-3; beta = 2; kappa = 0;
    lambda = alpha^2 * (n + kappa) - n;
    c = n + lambda;
    
    % 计算采样点
    sigma_points = zeros(n, 2*n+1);
    sigma_points(:,1) = X_est;
    sqrt_P = chol((n + lambda) * P, 'lower');
    
    for i = 1:n
        sigma_points(:, i+1)      = X_est + sqrt_P(:, i);
        sigma_points(:, i+1+n)    = X_est - sqrt_P(:, i);
    end
    
    % 重新计算权重
    Wm = [lambda/c, repmat(0.5/c, 1, 2*n)];
    Wc = Wm; 
    Wc(1) = Wc(1) + (1 - alpha^2 + beta);
    
    % ===== B. 时间更新（非线性状态转移）=====
    sigma_pred = zeros(size(sigma_points));
    for i = 1:size(sigma_points, 2)
        % 调用 SINS 非线性状态方程 f(x)
        sigma_pred(:, i) = f_SINS_state_update(sigma_points(:, i), imu_data(:, k), dt);
    end
    
    % 预测状态均值和协方差
    X_pred = sigma_pred * Wm';
    P_pred = Q;
    for i = 1:size(sigma_pred, 2)
        diff = sigma_pred(:, i) - X_pred;
        P_pred = P_pred + Wc(i) * (diff * diff');
    end
    
    % ===== C. 测量更新（含强跟踪渐消因子）=====
    % 1. 预测观测量
    Z_pred_sigma = zeros(m, 2*n+1);
    for i = 1:size(sigma_pred, 2)
        Z_pred_sigma(:, i) = h_SINS_measurement(sigma_pred(:, i));
    end
    Z_pred_mean = Z_pred_sigma * Wm';
    
    % 2. 计算新息协方差 Pzz 和互协方差 Pxz
    P_zz = R;
    P_xz = zeros(n, m);
    for i = 1:size(sigma_pred, 2)
        dz = Z_pred_sigma(:, i) - Z_pred_mean;
        dx = sigma_pred(:, i) - X_pred;
        P_zz = P_zz + Wc(i) * (dz * dz');
        P_xz = P_xz + Wc(i) * (dx * dz');
    end
    
    % *** 3. 【核心】计算强跟踪渐消因子 λ_k ***
    residual = ref_vel(:, k) - Z_pred_mean; % 当前时刻的新息(残差)
    % 简化的强跟踪因子计算（工程上可简化为检测残差突变）
    trace_Pxx = trace(P_pred);
    trace_Pvv = trace(R);
    rho = max(1, trace(residual * residual') / (trace_Pxx + trace_Pvv)); 
    lambda_k = rho; % 强跟踪渐消因子
    
    % 4. 应用渐消因子，强制调整协方差
    P_pred = lambda_k * P_pred;
    
    % 5. 计算卡尔曼增益并更新
    K = P_xz / P_zz;
    X_est = X_pred + K * residual;
    P = P_pred - K * P_zz * K';
    
    % 记录结果
    attitude_results(k, :) = X_est(1:3)'; % 记录失准角
end

%% 4. 绘图分析
figure;
plot(attitude_results(:, 3) * 57.3, 'LineWidth', 1.5); % 航向失准角转角度制
ylabel('航向失准角 (deg)');
xlabel('采样点数');
title('STUKF 初始对准收敛曲线');
grid on;

2. 核心非线性函数（需要你根据SINS力学编排编写）

matlab 复制代码

function state_next = f_SINS_state_update(state_curr, imu_meas, dt)
    % 此处应填入SINS在大失准角下的误差状态微分方程
    % phi_dot = ... + epsilon
    % dv_dot = ... + phi
    % epsilon_dot = 0
    % 为了演示，此处使用简化的欧拉法离散化
    F = eye(9); % 实际应从线性化矩阵中获取
    state_next = F * state_curr; 
end

function Z = h_SINS_measurement(state)
    % 观测方程：速度误差是直接观测到的
    Z = state(4:5); 
end

参考代码运用强跟踪UKF滤波实现捷联惯导实现初始对准 www.youwenfan.com/contentcst/54920.html

四、注意一下

强跟踪因子的调优 ：上面的代码提供了一个简化的 ρ\rhoρ 计算逻辑。在工业级实现中，强跟踪因子通常需要引入遗忘因子 或基于正交性原理 的严谨推导（如残差序列与状态预测应正交，若不正交则说明模型失配，需引入渐消因子）。MathWorks 官方文档和许多 IEEE 论文都提供了现成的 VkV_kVk 和 NkN_kNk 矩阵计算方法。
可观性问题 ：在静基座（完全静止）下，航向角（ϕU\phi_UϕU）通常是不可观的，必须要有线运动激励（如车辆轻微晃动、角运动）。STUKF 的优势在于能在这种激励不足的情况下，依旧能快速估计出部分失准角。
计算量管理 ：UKF 的计算量是 O(L3)O(L^3)O(L3)（L为Sigma点数），STUKF 又增加了求渐消因子的时间。如果在嵌入式芯片（如 STM32、FPGA）上跑，请务必对 chol（矩阵分解）和 sigma_points 的生成进行定点化优化。

总结来说 ，运用 STUKF 做 SINS 初始对准，就是用无迹变换 解决"大角度非线性"的难题，再用强跟踪渐消因子 解决"外界干扰导致发散"的痛点。只要将上述 Matlab 框架中的 f 和 h 函数替换为你实际的 SINS 误差模型，你就能立刻拥有一套抗干扰能力极强的初始对准算法。