企业官网建设流程全解析

做惯导的人迟早要碰一次传递对准。无论你是做弹载、机载还是车载组合导航，只要载体在运动状态下需要启动一套新的惯导系统，就绕不开这个问题：新装上去的子惯导初始姿态未知，又没法让载体停下来做静基座对准，怎么办？答案是借一套已经在正常工作的、精度更高的主惯导，在运动中把子惯导的姿态误差和器件误差估计出来。这个动作就叫传递对准。

我最早接触这个概念是在一个惯性导航系统的算法验证项目里，当时最头疼的不是滤波公式本身，而是缺一套能快速改参数、能复现、能对结果做定量评估的仿真平台。硬件转台不是随时都有，真实飞行的数据更是难得。所以我在MATLAB里从零搭了一套传递对准仿真程序，专门用来做算法预研、参数调优和可观测性分析。今天把整个程序的思路和关键实现完整写出来，希望能给正在被传递对准折磨的人一点参考。

这套程序能做什么，先交代清楚：

• 生成一条带机动的载体运动轨迹，输出真实的速度、姿态、位置；
• 模拟主惯导和子惯导的测量输出，包括陀螺、加速度计的噪声和常值误差；
• 人为注入初始失准角和器件误差，制造一个“已知真值的问题”；
• 跑速度匹配或姿态匹配的卡尔曼滤波，估计失准角和惯性器件误差；
• 把估计值和真值画在一起，直接看收敛情况。

适合谁来参考？正在做传递对准算法仿真、想重点验证卡尔曼滤波器设计、或者论文里需要传递对准对比结果的人。看完以后你自己就能搭起来，不需要依赖任何商业工具箱，只需要一个带基础MATLAB的环境。

1. 传递对准要解决的问题和仿真目标

1.1 为什么载体运动时不能简单做“静基座对准”

捷联惯导启动之后，第一步必须知道载体当前的姿态。静基座条件下，可以用解析法粗对准，也就是利用重力向量和地球自转角速度来确定水平姿态和航向。这个方法有一个隐含前提：载体是静止的，或者至少没有明显的线运动和角运动。但很多载体的工作环境不允许你停下来慢慢对准。

军舰甲板上的飞机、行驶中的车辆、飞行过程中需要重新启动的惯导系统，都处在持续运动状态。这时候主惯导已经正常工作，精度很高，子惯导刚上电，姿态一无所知。如果把主惯导的导航参数当作基准，让子惯导利用两者的速度和姿态差来反推自己的初始姿态误差，这就是传递对准的核心思路。

1.2 仿真的目标：验证算法，而不是等真机

真实系统里验证传递对准算法，需要高精度主惯导、转台、真实飞行任务，周期长成本高。而在MATLAB里搭一套纯数字仿真，可以在几分钟内换一组机动轨迹、换一组合噪声参数、换一种匹配方式，把算法的性能边界摸清楚。

我这套程序设定的典型任务是：在60秒内，将初始失准角[0.1°；0.1°；0.5°]的子惯导，通过速度匹配卡尔曼滤波，估计到0.01°量级以内。60秒只是一个参考窗口，实际收敛时间取决于机动幅度和噪声水平。程序里所有注入误差都设为已知真值，滤波器估计的结果可以直接和真值对比，收敛与否一目了然。

2. 建模前要把这三个模块的关系理清楚

写代码之前，第一件要搞清楚的事情是：传递对准仿真里的“误差模型”到底包含哪些模块。如果你直接把论文里的矩阵抄过来开跑，多半会翻车。我的建议是先理清楚下面三个模块，再动手写代码。

2.1 状态方程：失准角和速度误差怎么互相影响

传递对准的核心状态量，是子惯导相对主惯导的失准角φ和速度误差δV。这两者不是独立的，失准角会通过比力耦合进速度误差方程，而速度误差和姿态误差又会随运动学关系传递。状态向量取15维：

X = [φ_E, φ_N, φ_U, δV_E, δV_N, δV_U, ε_x, ε_y, ε_z, ∇_x, ∇_y, ∇_z]^T

其中ε是陀螺漂移，∇是加速度计零偏。在小失准角假设下，误差方程可以写成如下线性形式：

φ̇ = -ω_in^n × φ + C_b^n ε_b δV̇ = -(2ω_ie^n + ω_en^n) × δV + f^n × φ + C_b^n ∇_b

这就是滤波器的“状态传递”基础。第一个方程说的是失准角怎么随时间变化，一部分来自陀螺漂移的积分，一部分来自载体自身转动的耦合；第二个方程说的是速度误差不仅受加速度计零偏影响，还会因为失准角导致比力投影方向出错，产生一个f^n × φ的误差项。这一项是最关键的，正是它让卡尔曼滤波可以通过速度观测把失准角“反向”估计出来，这也是传递对准的原理所在。

在MATLAB里构建状态矩阵F时，只需要把上面的方程转成矩阵形式：

F = zeros(15, 15); F(1:3, 1:3) = -skew(omega_ie + omega_en); % 简化：忽略位置误差耦合 F(1:3, 7:9) = Cbn; % 陀螺漂移到失准角 F(4:6, 1:3) = skew(fn); % f^n × φ F(4:6, 4:6) = -skew(2*omega_ie + omega_en); % 哥氏项 F(4:6, 10:12) = Cbn; % 加速度计零偏到速度误差

2.2 量测方程：速度匹配和姿态匹配的差异

量测方程决定了滤波器“看到”什么。工程上最常用的有两种：

速度匹配。量测取主子惯导速度之差：

Z = V_sub - V_master = δV + v

H矩阵就是把状态中的δV取出来：

H = zeros(3, 15); H(1, 4) = 1; H(2, 5) = 1; H(3, 6) = 1;

姿态匹配。量测取主子惯导的姿态误差，在小角度假设下近似等于失准角：

Z = θ_sub - θ_master ≈ φ + v

程序里我建议做一个量测模式选择开关，这样你可以非常方便地对比两种匹配方式的性能差异：

use_att_measurement = false; % true=姿态匹配 false=速度匹配

初始化滤波器时对应切换H矩阵。

2.3 可观测性：为什么必须有机动

一开始我不理解，为什么仿真里经常出现某个失准角估计始终不动。后来才意识到，这不是滤波器写错了，而是可观测性问题。简单说，可观测性决定了“某个状态能不能从量测中恢复出来”。速度匹配模式下，水平失准角（φ_E、φ_N）可以通过比力与重力的耦合被观测，而航向失准角φ_U需要横向加速度的持续激励，也就是载体必须转弯或者侧向机动。如果你设计了一条匀速直线轨迹，航向失准角基本不可观测，滤波器给出的估计只会停留在初值附近。

所以轨迹设计里必须专门安排机动段。仿真结果好不好看，很大程度上在你设计轨迹的时候就已经决定了。

3. 轨迹生成和主子惯导数据模拟

3.1 轨迹剖面设计原则

传递对准仿真里，轨迹生成不是随便让载体动起来就行，它得照顾两个需求：一是让需要观测的状态被激励出来，二是贴近真实飞行包线。我常用的剖面是“直飞-转弯-直飞-转弯-直飞”三段式，总时长60秒，仿真步长0.01秒：

• 0~10s：匀速直飞，让滤波器进入稳定的传播阶段；
• 10~20s：以3°/s的偏航角速度左转，航向从30°转到60°；
• 20~30s：直飞，让滤波器消化转弯段带来的量测信息；
• 30~40s：以3°/s右转，航向转回30°；
• 40~60s：直飞收尾，观察估计是否能稳定保持。

除此之外，我会在三个姿态角上都叠一个幅度1°~2°的正弦小扰动，频率0.3~0.5Hz。这样做的目的是模拟真实载体持续存在的低幅值抖动，这种抖动对水平失准角的可观测性是有帮助的。注意不要把小扰动的幅度加太大，否则会超出线性误差模型的小角度假设，后面滤波容易出问题。

3.2 从真值到量测数据的生成流程

有了姿态角时间序列和速度剖面之后，按照下面的顺序生成数据就很清晰。

第一步，把欧拉角转成姿态矩阵。用ZYX旋转顺序，MATLAB里可以自己写euler2dcm函数，也可以用方向余弦矩阵的递推更新。初始姿态设定为：俯仰0°，横滚0°，航向30°。

第二步，生成速度真值。设地速恒定50m/s，按航向角分解到北东地坐标系：

Vn_true = [50*cos(yaw); 50*sin(yaw); zeros(1, N)];

第三步，由速度差分得到加速度，再算出比力真值f^n。捷联惯导中加速度计感受的是比力，而不是加速度。忽略小量时可以写成：

f^n ≈ V̇ + (2ω_ie^n + ω_en^n) × V - g^n

这个公式如果你一时不记得，记住物理含义就行：比力就是载体受到的非引力加速度。在静态水平状态下，加速度计输出的就是重力反方向的大小。

第四步，生成陀螺和加速度计测量值。给真值加上常值漂移和白噪声：

gyro_meas = gyro_true + gyro_bias + randn(3, N) * sigma_gyro; acc_meas = acc_true + acc_bias + randn(3, N) * sigma_acc;

3.3 误差注入：给子惯导人为制造初始失准角

这是仿真程序里最巧妙的一步。要让结果能定量评估，就必须“明知故犯”地注入已知误差，然后看滤波器能不能把它还原出来。我给子惯导设的初始失准角是：

φ_0 = [0.1°; 0.1°; 0.5°]

水平两个方向各0.1°，航向0.5°。这是很典型的传递对准任务场景：航向误差相对大一些。同时注入陀螺零偏0.01°/h、加速度计零偏100μg。

之后的做法是：不直接去跑完整的捷联解算，而是用误差方程把子惯导相对主惯导的误差随时间传播出来，作为“真实误差”，再叠加量测噪声形成量测Z。这样滤波器的估计结果可以直接和注入真值对比，一眼判断收敛情况。代码如下：

X_true = zeros(15, 1); X_true(1:3) = phi0; X_true(4:6) = [0.5; -0.3; 0.2]; % 初始速度误差 X_true(7:9) = gyro_bias; X_true(10:12) = acc_bias; for k = 1:N-1 F = buildF(fn(:, k), Cbn(:,:, k), omega_ie); X_true = (eye(15) + F * dt) * X_true; Z_true(:, k) = H * X_true; % 之后的量测就是用 Z_true + 噪声 end

这就把“滤波器估计值 vs 注入真值”的对比变得非常简单。

4. 卡尔曼滤波器从零搭建

4.1 离散化与矩阵构建

连续系统要先离散化才能编程。状态转移矩阵用二阶近似：

Phi = eye(15) + F * dt + 0.5 * F^2 * dt^2; Qd = Q * dt; % 简单近似的离散过程噪声

如果你的系统模型需要更精细，可以用矩阵指数函数，但对传递对准仿真来说，二阶近似已经足够，而且不容易出现数值发散。真正需要花心思的是F矩阵的构建函数，务必要保持单位一致。陀螺漂移用rad/s，加速度计零偏用m/s^2，不要混用度、角秒这些单位，否则隐蔽性极强。

4.2 初始化参数怎么定

卡尔曼滤波最让人头疼的不是公式推导，而是初始协方差P0和噪声阵Q、R的量级怎么给。我一开始把P0给得特别大，结果前几秒估计值疯狂跳；后来又把R给得很小，结果滤波器直接发散。调了几轮之后，我总结出一套比较稳的初值：

一个实用技巧：先把R设得特别大，让滤波器几乎只做状态预测，跑一次看状态会不会漂移；然后再逐步缩小R，观察收敛速度和噪声之间的平衡。如果R太小，新息会被噪声主导，状态估计会出现高频抖动。

4.3 滤波主循环和状态估计输出

滤波主循环其实很短，核心就是时间更新和量测更新两件事：

for k = 1:N-1 % 时间更新 X_pred = Phi * X_est; P_pred = Phi * P * Phi' + Qd; % 量测更新 Z = vs_sub(:, k) - vs_master(:, k); S = H * P_pred * H' + R; K = P_pred * H' / S; X_est = X_pred + K * (Z - H * X_pred); P = (eye(15) - K * H) * P_pred; phi_his(:, k+1) = X_est(1:3); dV_his(:, k+1) = X_est(4:6); end

程序跑完之后，绘图部分直接把估计的失准角曲线叠到注入真值上。记得把单位转换成角度或者角分，不然图上纵坐标的数字会非常难看。

5. 仿真结果怎么看、怎么调

5.1 收敛判断不能只看单点

不要盯着某个时间点的估计值就直接说“收敛了”。我判断一套仿真是否正常，主要看三个指标：

• 失准角估计误差是否进入稳态，且稳态值在设定性能范围内；
• 新息序列（Z - H*X_pred）是否在零附近波动，不出现明显偏置；
• P矩阵对角线是否随着收敛逐步减小，而不是振荡或发散。

这三个指标同时满足，才可以认为滤波器正常工作。

5.2 典型结果对照：转弯段决定航向失准角

以速度匹配为例，跑出来比较典型的规律是：在0~10s直飞段，水平失准角基本不动，航向失准角更是完全原地踏步；进入第一个转弯段之后，水平失准角和航向失准角同时快速收敛，大概10秒内就能收敛到0.01°量级；后面的右转段和直飞段只是让估计值进一步稳定。

如果你看到“直飞段航向失准角也在下降”这种结果，先别高兴，大概率是量测噪声设置得太小或者滤波器参数不当，属于假收敛。真正可靠的做法是故意把转弯段去掉跑一遍，这时候航向失准角应该明显不收敛，这就能证明你的可观测性设计是生效的。

5.3 发散排查链路

仿真跑飞了怎么排查？我比较推荐按下面的顺序一步步来：

第一，先查F矩阵里f^n × φ的符号方向。这是最容易出错的点，符号反了滤波器必发散； 第二，把量测噪声R加大，看状态是否会趋于稳定。如果R加大以后反而不发散，说明原来的R太小，量测噪声被滤波当成真实信号用了； 第三，检查新息序列。如果新息一直有固定偏置，说明注入误差的真实演化过程和滤波器的模型不一致。这时候重点检查状态转移矩阵和陀螺、加速度计零偏的单位换算； 第四，如果失准角估计收敛到某个偏置值下不来，大概率是陀螺漂移不可观测，或者注入的某轴漂移没有被激励起来。这种情况不是调参能解决的，要从轨迹机动上找改善空间。

6. 工程仿真中容易被忽略的细节

6.1 杆臂效应会让速度量测产生系统性偏差

主子惯导装在同一个载体上，不代表它们感受的运动完全一样。只要安装位置不重合，载体转动时两个位置的速度就不同，这个速度差叫杆臂速度。速度匹配量测里如果不补偿，就会给滤波器引入系统偏置，最终表现为失准角估计有稳态误差。仿真里模拟杆臂效应很简单，只需要在量测生成时加一项：

lever_arm = [2; 0.2; 0.3]; % 子惯导相对主惯导的杆臂，单位米 V_lever = Cbn * cross(gyro_true, lever_arm); % 载体转动引起的杆臂速度

真实系统里还需要补偿主惯导本身的运动速度差，这里只是示意。至少要明白：速度匹配不是简单的“两个速度相减”就完事。

6.2 时间同步和数据率的影响比你想象的大

仿真里通常会忽略时间同步问题，因为所有数据都在同一时刻生成。真实系统中这往往是最大的坑：主惯导和子惯导的输出频率不一致，信号经过通讯链路还有延迟。姿态匹配对时间延迟尤其敏感，因为角度随时间在快速变化，延迟几十毫秒就可能引入明显误差。仿真中想验证这个影响，可以把量测数据人为平移几个采样周期，看滤波结果会不会变差。我试过，仅仅延迟0.05秒，航向失准角估计的稳态误差就能从0.01°恶化到0.1°量级。

6.3 线性模型的适用范围要当成硬约束

最后说一个很多人容易忽视的前提。这套程序基于小失准角线性误差模型，要求初始失准角在几度以内。如果应用场景里初始失准角很大，比如航向完全未知，那线性卡尔曼是扛不住的，你需要的是大失准角模型或非线性滤波。仿真中我的做法是：给程序加一个前置检查，如果注入的初始失准角超过5°，直接提示算法适用性不足，免得后面调参半天结果还是不对。

我自己跑这套程序时最大的体会是：传递对准仿真最珍贵的不是“能跑通”，而是“能对比”。由于注入了真值，每一次参数修改之后，你都能明确看到收敛是变好了还是变坏了，这对理解可观测性、调滤波器参数都有非常大的帮助。后面如果你在这个基础上做扩展，可以尝试加入里程计或者多普勒雷达辅助的传递对准、考虑挠曲变形的动态传递对准，或者把可观测度分析做成实时曲线，都是很有价值的方向。