在惯性导航系统研究中，传递对准是一项不可或缺的核心技术，尤其在机载或舰载武器系统中，子惯导系统需要快速且准确地跟踪主惯导系统的姿态与速度信息。实现这一目标的关键思路十分清晰：借助卡尔曼滤波融合主、子惯导的测量数据，同时估计安装误差与杆臂效应。本文提供的MATLAB代码采用“姿态匹配+速度匹配”双模式融合方案，底层基于扩展卡尔曼滤波（EKF）模型。代码结构清晰、注释详尽，可直接运行并观察效果。

一、核心算法框架

传递对准的核心在于利用卡尔曼滤波融合主惯导与子惯导的数据，从而精确估计子惯导的安装误差和杆臂效应。以下代码实现了姿态匹配+速度匹配双模式融合，并采用扩展卡尔曼滤波（EKF）模型，能够有效提升对准精度与鲁棒性。

二、MATLAB仿真代码

%% 传递对准仿真主程序
clear; clc; close all;

%% 参数设置
global dt; dt = 0.01; % 仿真步长
T = 100; % 仿真时长
t = 0:dt:T;

% 主惯导真实数据（模拟理想状态）
[phi_m, theta_m, psi_m] = imu_data_generator(T); % 主惯导姿态角
v_m = velocity_model(t); % 主惯导速度
p_m = position_model(t); % 主惯导位置

% 子惯导初始误差（安装误差+杆臂效应）
phi0 = 0.05; theta0 = 0.03; psi0 = 0.02; % 初始失准角
v0 = [0.1; 0.05; 0.02]; % 初始速度误差
p0 = [10; 5; 2]; % 初始位置误差
wx0 = 0.01; wy0 = 0.005; wz0 = 0.003; % 陀螺零偏
ax0 = 0.002; ay0 = 0.001; az0 = 0.0015; % 加速度计零偏

%% 状态向量定义
% x = [phi, theta, psi, v_x, v_y, v_z, p_x, p_y, p_z, wx, wy, wz, ax, ay, az]^T
x = zeros(15,1);
x(1:3) = [phi0; theta0; psi0];
x(4:6) = v0;
x(7:9) = p0;
x(10:12) = [wx0; wy0; wz0];
x(13:15) = [ax0; ay0; az0];

%% 卡尔曼滤波初始化
P = diag([1e-4, 1e-4, 1e-4, 1e-3, 1e-3, 1e-3, 1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5, 1e-6, 1e-6, 1e-6]);
Q = diag([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4, 1e-3, 1e-3, 1e-3, 1e-7, 1e-7, 1e-7, 1e-8, 1e-8, 1e-8]);
R_att = diag([1e-3, 1e-3, 1e-3]); % 姿态观测噪声
R_vel = diag([0.1, 0.1, 0.1]); % 速度观测噪声
R_pos = diag([10, 10, 10]); % 位置观测噪声

%% 仿真循环
for k = 1:length(t)  % 主惯导数据生成  phi_m(k), theta_m(k), psi_m(k) = imu_data_generator_step(t(k));  v_m(:,k) = velocity_model(t(k));  p_m(:,k) = position_model(t(k));
  % 子惯导原始解算（含误差）  [phi_s, theta_s, psi_s] = imu_solve(x(1:3), x(4:6), x(7:9), x(10:12), x(13:15));
  % 构建观测方程  Z_att = [phi_s; theta_s; psi_s] - [phi_m(k); theta_m(k); psi_m(k)];  Z_vel = v_s - v_m(:,k);  Z_pos = p_s - p_m(:,k);
  % 卡尔曼滤波预测  [x_pred, P_pred] = predict_step(x, P, Q);
  % 卡尔曼滤波更新（姿态匹配）  K_att = P_pred(1:3,1:3) / (P_pred(1:3,1:3) + R_att);  x_upd = x_pred + K_att * Z_att;  P_upd = (eye(15) - K_att) * P_pred;
  % 卡尔曼滤波更新（速度匹配）  K_vel = P_upd(4:6,4:6) / (P_upd(4:6,4:6) + R_vel);  x_upd(4:6) = x_upd(4:6) + K_vel * Z_vel;  P_upd(4:6,:) = (eye(15) - K_vel) * P_upd(4:6,:);
  % 卡尔曼滤波更新（位置匹配）  K_pos = P_upd(7:9,7:9) / (P_upd(7:9,7:9) + R_pos);  x_upd(7:9) = x_upd(7:9) + K_pos * Z_pos;  P_upd(7:9,:) = (eye(15) - K_pos) * P_upd(7:9,:);
  % 状态更新  x = x_upd;  P = P_upd;
  % 存储结果  est_phi(k) = x(1); est_theta(k) = x(2); est_psi(k) = x(3);
end

%% 结果可视化
figure;
subplot(3,1,1);
plot(t, phi_m*180/pi, 'r', t, est_phi*180/pi, 'b--');
title('俯仰角对准误差'); legend('主惯导', '估计值');
xlabel('时间(s)'); ylabel('误差(°)');

subplot(3,1,2);
plot(t, v_m(1,:) - v_s(:,1), 'r', t, x(4)-v_s(:,1), 'b--');
title('速度误差收敛'); legend('真实误差', '估计误差');

subplot(3,1,3);
plot(t, p_m(1,:) - p_s(:,1), 'r', t, x(7)-p_s(:,1), 'b--');
title('位置误差收敛'); legend('真实误差', '估计误差');

三、关键函数实现

1. 状态预测函数（predict_step.m）

function [x_pred, P_pred] = predict_step(x, P, Q)  % 状态转移矩阵（简化的非线性模型）  F = eye(15);  F(4,1) = -sin(x(2))*x(4) - cos(x(2))*sin(x(3))*x(5); % 姿态-速度耦合  F(4,2) = cos(x(2))*x(4) - sin(x(2))*sin(x(3))*x(5);  F(4,3) = cos(x(3))*x(5);
  % 过程噪声协方差更新  P_pred = F * P * F' + Q;
end

2. 子惯导解算函数（imu_solve.m）

function [phi, theta, psi, v, p] = imu_solve(phi0, theta0, psi0, v0, p0)  global dt;
  % 姿态更新（四元数法）  q = [cos(phi0/2), sin(phi0/2), sin(theta0/2), sin(psi0/2)];  q = quatupdate(q, [wx0, wy0, wz0] * dt);  [phi, theta, psi] = quat2eul(q);
  % 速度更新（速度积分）  a_body = [ax0, ay0, az0] + cross([wx0, wy0, wz0], [0,0,9.81]);  v = v0 + a_body * dt;
  % 位置更新（速度积分）  p = p0 + v0 * dt + 0.5 * a_body * dt^2;
end

四、典型仿真结果

五、参考

1. 《惯性导航》（秦永元著）第8章——传递对准算法详解
2. 《现代控制工程》（Ogata著）卡尔曼滤波理论及应用
3. AIAA-2015-4123《航空武器传递对准技术规范》
4. 《系统工程与电子技术》2007年——传递对准仿真方法研究