【MATLAB例程】基于EKF的二维组合导航仿真代码，状态量为位置、速度、航向角与IMU偏置，观测量为XY轴的位置和速度，附代码下载链接

8维状态量（2维位置、2维速度、航向角、航向角偏置、2维加速度计偏置）+4维观测量（2维位置、2维速度）。

程序详解

程序概述

本例程实现了 基于 8 维误差状态模型的扩展卡尔曼滤波 (EKF)，用于 IMU 与 GNSS 的组合导航。
状态向量为：

$$x={\left[\begin{array}{cccccccc}{p}_x& p_y& v_x& v_y& \psi & b_g& b_{ax}& b_{ay}\end{array}\right]}^T$$

分别表示位置、速度、航向角、陀螺零偏、加速度计零偏。

系统主要功能包括：

1. 真实轨迹生成（圆轨迹运动，角速度已知）；
2. IMU 模拟积分（仅用 IMU 数据进行位置速度积分，作为对比）；
3. EKF 滤波器实现：状态预测 + 观测更新；
4. GNSS观测引入（位置与速度观测，每秒更新一次）；
5. 滤波结果可视化与误差统计（轨迹对比、误差曲线、RMSE）。

EKF 预测与更新

• 协方差预测
• 观测方程（GNSS 提供位置和速度）
• 卡尔曼增益
• 状态与协方差更新

核心代码片段

1. 状态转移

x_next(5) = x(5) + (imu(3) - x(6)) * dt;           % 航向角更新
C_bn = euler_to_dcm(0,0,x(5));
f_corrected = imu(1:2) - x(7:8);
x_next(3:4) = x(3:4) + C_bn(1:2,1:2) * f_corrected * dt;  % 速度更新
x_next(1:2) = x(1:2) + x(3:4) * dt;                       % 位置更新

2. 雅可比矩阵

F = eye(8);
F(1,3) = dt; F(2,4) = dt;                   % 位置对速度
df_dyaw = [-sin(yaw), -cos(yaw); cos(yaw), -sin(yaw)] * f * dt;
F(3:4,5) = df_dyaw;                          % 速度对航向角
F(3:4,7:8) = -C(1:2,1:2) * dt;              % 速度对加计偏差
F(5,6) = -dt;                               % 航向角对陀螺偏差

3. 观测模型

H = zeros(4,8);
H(1:2,1:2) = eye(2); % 位置
H(3:4,3:4) = eye(2); % 速度

运行结果

定位导航轨迹示意图：

各方法得到的XY两个轴的位移-时间图像：

各方法得到的XY两各轴的速度-时间图像：

各方法得到的位置误差与速度误差：

误差统计特性：

MATLAB源代码

完整代码如下：

% 二维状态量的EKF例程（有严格的组合导航推导）
% 基于8维误差状态模型：位置、速度、航向、航向角角速度偏差、加速度计偏差
% 作者：matlabfilter
% 2025-08-27/Ver1

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

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

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

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

%% 过程噪声协方差矩阵Q (8×8)

完整代码的下载链接：

https://download.youkuaiyun.com/download/callmeup/91777849

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