Engee 中的 INS 和 GNSS 的 Kalman Filter

Engee中INS/GNSS融合的最优卡尔曼滤波器

无人飞行器（UAV）依靠惯性导航系统（INS）与卫星导航系统（GNSS）的结合，精准获取运动参数。最优卡尔曼滤波器利用GNSS数据校正INS累积误差，实现高精度的位置与速度更新。该算法在Engee仿真环境中实现，离散步长为0.1秒。

INS因陀螺仪与加速度计误差、初始条件偏差及计算漂移，在扰动条件下运行。惯性导航的核心方程为：R' = V_n + g(R) + n，其中R为位置矢量，V_n为视速度，g为重力梯度。

GNSS定位精度可达10–12米，速度精度达0.05 m/s，差分模式下可提升至1–2米。主要误差来源包括星历不准确、电离层与对流层延迟以及接收机噪声。

初始化与仿真参数

飞行轨迹模拟高度为1,000米，路径包含转向点：纬度 [55°, 56.58°, 56.46°, 56.96°]，经度 [37°, 37.5°, 38°, 38.5°]。使用常数：Rz = 6,371,000 m，g = 9.78049 m/s²，dt = 0.1 s，t = 3,600 s。

初始误差：

• λ_error = 6/Rz（经度）
• φ_error = 6/Rz（纬度）
• V_e_error = 0.05 m/s
• V_n_error = 0.05 m/s
• ψ_error = 0.25°（航向）
• γ_error = 0.03°（滚转）
• θ_error = 0.03°（俯仰）

数组初始化代码：

X_out=zeros(19,1);
w = zeros(19,1);
sigma = zeros(19,1);
D = zeros(19,1);
psi = zeros(19,1);
Fi = zeros(36001,1);
lam = zeros(36001,1);
# ... (其他数组)

转向点定义：

fi_o = [55, 56.58, 56.46, 56.96].*pi/180;
lam_o = [37, 37.5, 38, 38.5].*pi/180;
H = 1000;

大圆航线计算

大圆航线是球面上最短路径。对每一段路径，计算如下内容：

• 经度变化：Δλ = λ_{j} - λ_{j-1}
• 大圆距离：σ = acos(sin φ_{j-1} sin φ_j + cos φ_{j-1} cos φ_j cos Δλ)
• 实际距离：D = Rz ⋅ σ
• 航向角：ψ = atan(cos φ_j sin Δλ, cos φ_{j-1} sin φ_j - sin φ_{j-1} cos φ_j cos Δλ)

航向ψ归一化至[0, 2π]区间。

总距离 sum(D)，平均速度 V = sum(D)/t。

航向计算代码：

for j = 2:1:4
    w[j-1]=lam_o[j]-lam_o[j-1];
    sigma[j-1]=acos(sin(fi_o[j-1])*sin(fi_o[j])+cos(fi_o[j-1])*cos(fi_o[j])*cos(w[j-1]));
    D[j-1]=Rz*sigma[j-1];
    psi[j-1]=atan(cos(fi_o[j])*sin(w[j-1]),cos(fi_o[j-1])*sin(fi_o[j])-sin(fi_o[j-1])*cos(fi_o[j])*cos(w[j-1]));
    if (psi[j-1] <= 0) psi[j-1] = psi[j-1] + 2*pi; end
    if (psi[j-1] > 2*pi) psi[j-1] = psi[j-1] - 2*pi; end
end

左右转向轨迹构建（LUR）

LUR（左/右转向）：转弯半径 Rr = V²/(g tan γ)，时间 Tr = Rr ⋅ Δψ / V，附加距离 lur_dist = Rr tan(0.5 Δψ)。

速度投影：V_e = V sin ψ（东向），V_n = V cos ψ（北向）。角速率：w_E = V_n/Rz，w_N = V_e/Rz。

增量计算：dφ = w_E dt，dλ = (w_N / cos φ) dt。

直线段与LUR的仿真循环确保轨迹连续无间断。

离散卡尔曼滤波器

动态模型：X' = F X + G W，Z = H X + V。

离散形式：

X̃_k = Φ X_{k-1} + Γ w_{k-1}

P̃_k = Φ P_{k-1} Φᵀ + Q

K_k = P̃_k Hᵀ (H P̃_k Hᵀ + R)^{-1}

X_k = X̃_k + K_k (Z_k - H X̃_k)

P_k = (I - K_k H) P̃_k

其中Φ为状态转移矩阵，Q与R分别代表过程噪声与测量噪声协方差。

Engee中的实现与误差建模

滤波循环模拟陀螺仪与加速度计的传感器读数，包含零偏、比例因子误差和随机噪声：

• 陀螺仪：零偏误差 = 0.001°/s，系数误差 = 2×10⁻⁵，噪声 = 0.01×零偏×randn()
• 加速度计：零偏误差 = g×60×10⁻⁶，系数误差 = 3×10⁻⁶，噪声类似

片段代码：

for i=1:1:length(lam)-1
    i=i+1;
    # 陀螺仪
    x_gyro_zero_error=0.001*pi/180;
    x_gyro_noise=x_gyro_zero_error*0.01*randn(1);
    x_gyro_koef_error=2*10^-5;
    # 加速度计
    x_ac_zero_error=g*60*(10^-6);
    x_ac_noise=x_ac_zero_error*0.01*randn(1);
    x_ac_koef_error=3*10^-6;
    # ...
end

滤波器更新一个19维状态向量 X_out，轨迹数据存储于 X_out_list，协方差矩阵保存于 Pk_list。

核心要点

• INS/GNSS融合：卡尔曼滤波器通过卫星数据消除惯性系统的漂移积累。
• 大圆航线：基于公式 Rr = V²/(g tan γ) 实现精确的左右转向计算。
• 误差建模：真实模拟陀螺仪漂移（0.001°/s）与加速度计零偏（60 μg）。
• 离散实现：0.1秒步长，状态转移矩阵Φ由线性化动力学推导得出。
• Engee脚本：完整代码库支持复现集成导航系统。

— Editorial Team