【MATLAB例程】三维组合导航，滤波使用EKF，带严格的惯导推算、雅克比求解函数，图像对比滤波前后的速度、位置、姿态

程序介绍

本程序实现了 三维状态量的扩展卡尔曼滤波（EKF）组合导航仿真，采用严格的15维误差状态模型，状态向量包括：

$$x={\left[\begin{array}{ccccccccccccccc}{p}_x& p_y& p_z& v_x& v_y& v_z& \varphi & \theta & \psi & b_g^x& b_g^y& b_g^z& b_a^x& b_a^y& b_a^z\end{array}\right]}^T$$

其中，位置 $p$、速度 $v$、姿态欧拉角 $(\varphi ,\theta ,\psi )$、陀螺偏差 $b_g$、加速度计偏差 $b_a$ 构成完整的15维误差状态。

系统建模

• 过程模型 由IMU观测驱动，采用离散状态转移函数：

$$\begin{array}{rl}{p}_{k+1}& =p_k+v_k\Delta t\\ v_{k+1}& =v_k+(C_{bn}(f_m-b_a)-g)\Delta t\\ {\Theta }_{k+1}& ={\Theta }_k+({\omega }_m-b_g)\Delta t\\ b_g,b_a& \text{视为随机游走模型}\end{array}$$

其中，$f_m,{\omega }_m$ 分别为测得的加速度与角速度，$C_{bn}$ 为姿态方向余弦矩阵。

• 观测模型 来自GNSS，观测量为位置和速度

滤波框架

扩展卡尔曼滤波按以下步骤实现：

• 预测：

$${\hat{x}}_{k|k-1}=f({\hat{x}}_{k-1},u_{k-1})$$

$$P_{k|k-1}=F{P}_{k-1}{F}^T+Q$$

• 更新：
  若有GNSS观测：

$$K_k=P_{k|k-1}{H}^T(H{P}_{k|k-1}{H}^T+R{)}^{-1}$$

$${\hat{x}}_k={\hat{x}}_{k|k-1}+K_k(z_k-h({\hat{x}}_{k|k-1}))$$

$$P_k=(I-K_{k}H)P_{k|k-1}$$

其中，$F$ 和 $H$ 分别为系统雅可比矩阵和观测雅可比矩阵。

仿真设置

• 真实轨迹为 螺旋上升运动，即圆周运动叠加线性爬升。
• IMU数据由真实运动加噪声和随机偏差生成。
• GNSS每秒输出一次位置与速度观测。

性能对比

程序对比了三种轨迹：

1. 真实轨迹（蓝线）
2. 纯IMU积分结果（红线）
3. EKF融合结果（黑线）
   并绘制位置、速度、姿态曲线及误差随时间的变化，计算均方根误差 (RMSE) 作为性能指标。

代码优点

• 使用严格的 15维误差状态建模，保证了惯导/GNSS组合导航的一致性推导。
• 包含 状态转移雅可比矩阵 和 观测雅可比矩阵 的显式计算，便于理论分析与扩展。
• 程序结果直观展示了 EKF在三维导航中的精度改进：有效抑制纯IMU积分的发散，显著降低位置与速度误差。

运行结果

三维轨迹对比：

各轴速度、位置、姿态对比：

误差对比：

MATLAB源代码

部分代码：

% 三维状态量的EKF例程（严格的组合导航推导）
% 基于15维误差状态模型：位置(3)、速度(3)、姿态(3)、陀螺偏差(3)、加速度计偏差(3)
% 作者：matlabfilter
% 2025-08-25/Ver1 

clear; clc; close all;
rng(0); % 固定随机种子

%% 系统参数设置
dt = 0.1;           % 采样时间间隔 (s)
total_time = 100;   % 总仿真时间 (s)
N = total_time / dt; % 采样点数

%% 噪声参数设置
% IMU噪声参数
gyro_noise_std = 0.01 * pi/180;      % 陀螺噪声标准差 (rad/s)
accel_noise_std = 0.001;             % 加速度计噪声标准差 (m/s^2)
gyro_bias_std = 0.001 * pi/180;      % 陀螺偏差标准差 (rad/s)
accel_bias_std = 0.0001;             % 加速度计偏差标准差 (m/s^2)

% GNSS观测噪声
gnss_pos_noise_std = 3;             % GNSS位置噪声标准差 (m)
gnss_vel_noise_std = 0.1;           % GNSS速度噪声标准差 (m/s)

%% 过程噪声协方差矩阵Q (15×15)
% 状态顺序：[位置(3), 速度(3), 姿态(3), 陀螺偏差(3), 加速度计偏差(3)]
Q = zeros(15, 15);
% 位置噪声（通过速度积分产生）
Q(1:3, 1:3) = eye(3) * (accel_noise_std * dt^2 / 2)^2;
% 速度噪声
Q(4:6, 4:6) = eye(3) * (accel_noise_std * dt)^2;
% 姿态噪声
Q(7:9, 7:9) = eye(3) * (gyro_noise_std * dt)^2;
% 陀螺偏差噪声
Q(10:12, 10:12) = eye(3) * (gyro_bias_std * dt)^2;
% 加速度计偏差噪声
Q(13:15, 13:15) = eye(3) * (accel_bias_std * dt)^2;

完整代码：

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者