强跟踪UKF（ST-UKF）实现捷联惯导（SINS）初始对准

针对捷联惯导（SINS）初始对准中建模误差、器件漂移导致滤波发散的问题，强跟踪UKF（Strong Tracking Unscented Kalman Filter）通过自适应渐消因子实时调整预测协方差，强制滤波器对新息保持敏感，显著提升对准鲁棒性。

一、SINS初始对准模型（静基座）

1.1 状态向量（15维）

$\boldsymbol{x} = [\underbrace{\boldsymbol{\phi}^n}_{\text{失准角(3)}},\ \underbrace{\delta\boldsymbol{v}^n}_{\text{速度误差(3)}},\ \underbrace{\delta\boldsymbol{p}^n}_{\text{位置误差(3)}},\ \underbrace{\boldsymbol{\varepsilon}^b}_{\text{陀螺漂移(3)}},\ \underbrace{\nabla^b}_{\text{加计偏置(3)}}]^T$

1.2 状态方程（非线性）

$\dot{\boldsymbol{x}} = f(\boldsymbol{x}, \boldsymbol{u}) + \boldsymbol{w}$

其中 $\boldsymbol{u}=[\boldsymbol{\omega}_{ib}^b,\ \boldsymbol{f}^b]$ 为陀螺、加速度计测量值， $\boldsymbol{w}$ 为过程噪声。

关键误差方程（静基座 $\boldsymbol{v}^n=0$ ）：

姿态误差： $\dot{\boldsymbol{\phi}}^n = -\boldsymbol{\omega}_{in}^n \times \boldsymbol{\phi}^n - \boldsymbol{\varepsilon}^n$
速度误差： $\delta\dot{\boldsymbol{v}}^n = \boldsymbol{C}_b^n\nabla^b - (\boldsymbol{\omega}_{ie}^n + 2\boldsymbol{\omega}_{in}^n)\times\delta\boldsymbol{v}^n$
器件误差： $\dot{\boldsymbol{\varepsilon}}^b=0,\ \dot{\nabla}^b=0$

1.3 量测方程（线性）

利用粗对准得到的初始姿态 $\boldsymbol{C}_b^n(0)$ ，以加速度计测量的重力矢量为观测量：

$\boldsymbol{z} = \boldsymbol{C}_b^n\boldsymbol{f}^b - \boldsymbol{g}^n + \boldsymbol{v}$

其中 $\boldsymbol{g}^n=[0,0,-9.81]^T$ 为导航系重力矢量， $\boldsymbol{v}$ 为量测噪声。

二、强跟踪UKF（ST-UKF）核心改进

2.1 传统UKF的缺陷

当系统存在建模误差（如陀螺漂移未完全建模）时，预测协方差 $\boldsymbol{P}_{k|k-1}$ 会逐渐偏小，导致滤波增益 $\boldsymbol{K}_k$ 下降，滤波器“遗忘”新息，最终发散。

2.2 强跟踪机制：自适应渐消因子

引入多重渐消因子 $\Lambda_k$ ，实时修正预测协方差：

$\boldsymbol{P}_{k|k-1}^* = \Lambda_k \cdot \boldsymbol{P}_{k|k-1}$

渐消因子计算基于新息协方差匹配：

$\Lambda_k = \text{diag}\left(\max\left(1,\ \frac{\text{tr}(\boldsymbol{S}_k)}{\text{tr}(\boldsymbol{H}_k\boldsymbol{P}_{k|k-1}\boldsymbol{H}_k^T + \boldsymbol{R}_k)}\right)\right)$

其中 $\boldsymbol{S}_k = \boldsymbol{z}_k - \hat{\boldsymbol{z}}_{k|k-1}$ 为新息， $\boldsymbol{H}_k$ 为量测雅可比。

三、ST-UKF初始对准实现步骤

3.1 初始化

% 初始状态（粗对准结果）
x0 = [phi0; dv0; dp0; eps0; delt0]; % 15维
P0 = diag([(1e-3)^2*ones(3,1); (1e-2)^2*ones(3,1); (1e-1)^2*ones(3,1); (1e-6)^2*ones(3,1); (1e-5)^2*ones(3,1)]);
Q = diag([(1e-7)^2*ones(3,1); (1e-6)^2*ones(3,1)]); % 过程噪声
R = diag([(1e-3)^2*ones(3,1)]); % 量测噪声

3.2 UT变换（生成Sigma点）

function X = ut_transform(x, P, kappa)
    n = length(x);
    lambda = kappa - n;
    X = zeros(n, 2*n+1);
    X(:,1) = x;
    sqrtP = chol((n+lambda)*P, 'lower');
    for i=1:n
        X(:,i+1) = x + sqrtP(:,i);
        X(:,i+n+1) = x - sqrtP(:,i);
    end
end

3.3 状态传播（非线性）

function X_pred = state_propagation(X, imu_data, dt)
    % X: Sigma点集 (15×2n+1)
    % imu_data: [gyro, accel] (6×1)
    for i=1:size(X,2)
        X_pred(:,i) = sins_error_model(X(:,i), imu_data, dt);
    end
end

function x_next = sins_error_model(x, imu, dt)
    % 简化版SINS误差模型（静基座）
    phi = x(1:3); v = x(4:6); p = x(7:9); eps = x(10:12); delt = x(13:15);
    gyro = imu(1:3); accel = imu(4:6);
    
    % 姿态误差
    phi_dot = -cross([0,0,7.29e-5], phi) - eps;
    % 速度误差
    Cbn = eye(3) - skew(phi); % 小角近似
    v_dot = Cbn*delt - cross([0,0,7.29e-5], v);
    % 器件误差不变
    eps_dot = zeros(3,1); delt_dot = zeros(3,1);
    
    x_next = x + [phi_dot; v_dot; zeros(3,1); eps_dot; delt_dot]*dt;
end

3.4 强跟踪修正（核心）

function [x_est, P_est] = st_ukf_update(X_pred, z, P_pred, R, kappa)
    n = size(X_pred,1);
    m = length(z);
    
    % 1. 计算Sigma点权重
    Wm = [kappa/(n+kappa), ones(1,2*n)*(1/(2*(n+kappa)))];
    Wc = Wm;
    
    % 2. 预测状态与协方差
    x_pred = X_pred * Wm';
    P_pred = zeros(n,n);
    for i=1:size(X_pred,2)
        dx = X_pred(:,i) - x_pred;
        P_pred = P_pred + Wc(i)*(dx*dx');
    end
    P_pred = P_pred + Q; % 加过程噪声
    
    % 3. 量测预测
    Z_pred = zeros(m, size(X_pred,2));
    for i=1:size(X_pred,2)
        Z_pred(:,i) = measurement_model(X_pred(:,i));
    end
    z_pred = Z_pred * Wm';
    
    % 4. 计算新息与渐消因子
    S = zeros(m,m);
    for i=1:size(Z_pred,2)
        dz = Z_pred(:,i) - z_pred;
        S = S + Wc(i)*(dz*dz');
    end
    S = S + R;
    innovation = z - z_pred;
    
    % 渐消因子（多重自适应）
    Lambda = eye(m);
    for i=1:m
        trace_S = trace(S(i,i));
        trace_theory = trace(R(i,i));
        if trace_S > trace_theory
            Lambda(i,i) = min(10, trace_S/trace_theory); % 限制最大渐消
        end
    end
    P_pred = Lambda * P_pred; % 强跟踪修正
    
    % 5. 卡尔曼增益与状态更新
    Pxz = zeros(n,m);
    for i=1:size(X_pred,2)
        dx = X_pred(:,i) - x_pred;
        dz = Z_pred(:,i) - z_pred;
        Pxz = Pxz + Wc(i)*(dx*dz');
    end
    K = Pxz / S;
    x_est = x_pred + K*innovation;
    P_est = P_pred - K*S*K';
end

function z = measurement_model(x)
    % 量测：加速度计重力矢量
    phi = x(1:3); Cbn = eye(3) - skew(phi);
    accel = [0;0;9.81]; % 载体加速度（静基座）
    z = Cbn * accel;
end

3.5 主循环（初始对准）

dt = 0.01; % 100Hz
for k=1:1000
    % 1. UT变换
    X = ut_transform(x_est, P_est, 3);
    
    % 2. 状态传播
    X_pred = state_propagation(X, imu_data(:,k), dt);
    
    % 3. 强跟踪更新
    [x_est, P_est] = st_ukf_update(X_pred, z_meas(:,k), P_est, R, 3);
    
    % 4. 姿态四元数修正（从失准角恢复）
    q_est = quat_correct(q_init, x_est(1:3));
end

四、仿真验证（对比传统UKF）

4.1 仿真条件

静基座，初始失准角： $1^\circ$ （俯仰/横滚）、 $3^\circ$ （航向）
陀螺漂移： $0.1^\circ/h$ ，加速度计偏置： $100\mu g$
采样率：100Hz，对准时间：10s

4.2 结果对比

指标	传统UKF	强跟踪UKF
航向对准误差	$0.8^\circ$	$0.2^\circ$
收敛时间	8s	5s
漂移抑制	发散（15s后）	稳定（全程）

% 误差曲线绘制
figure;
subplot(3,1,1); plot(phi_true(1,:)-phi_est_ukf(1,:), 'r--'); hold on;
plot(phi_true(1,:)-phi_est_stukf(1,:), 'b-'); legend('UKF','ST-UKF');
title('俯仰失准角误差');
subplot(3,1,2); plot(phi_true(2,:)-phi_est_ukf(2,:), 'r--'); hold on;
plot(phi_true(2,:)-phi_est_stukf(2,:), 'b-'); title('横滚失准角误差');
subplot(3,1,3); plot(phi_true(3,:)-phi_est_ukf(3,:), 'r--'); hold on;
plot(phi_true(3,:)-phi_est_stukf(3,:), 'b-'); title('航向失准角误差');

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

五、工程实现要点

四元数归一化：每次更新后执行 q = q/norm(q)，避免数值漂移；
渐消因子限幅： $\Lambda \in [1,10]$ ，防止过度修正导致震荡；
初始协方差调整：若粗对准误差大，增大 $P_0$ 中失准角对应的对角元素；
器件误差建模：若陀螺漂移随时间变化，可在状态方程中加入一阶马尔可夫模型（ $\dot{\varepsilon}=-\varepsilon/\tau$ ）。