程序详解

概述

本代码实现基于扩展卡尔曼滤波器（EKF）的二维组合导航系统，融合IMU（惯性测量单元）和GNSS（全球导航卫星系统）数据，实现精确的位置和速度估计。该系统采用8维误差状态模型，在圆形轨迹上进行仿真验证。

系统架构

状态向量定义

系统采用8维状态向量：

$$\mathbf{x}=\left[\begin{array}{c}{p}_x\\ p_y\\ v_x\\ v_y\\ \psi \\ b_g\\ b_{ax}\\ b_{ay}\end{array}\right]$$

其中：

• $p_x,p_y$：X、Y方向位置 (m)
• $v_x,v_y$：X、Y方向速度 (m/s)
• $\psi$：航向角 (rad)
• $b_g$：陀螺仪偏差 (rad/s)
• $b_{ax},b_{ay}$：X、Y方向加速度计偏差 (m/s²)

观测向量定义

观测向量为2维GNSS位置观测：

$$\mathbf{z}=\left[\begin{array}{c}{p}_x\\ p_y\end{array}\right]$$

核心数学模型

状态转移方程

系统的非线性状态转移方程为：

$${\mathbf{x}}_{k+1}=f({\mathbf{x}}_k,{\mathbf{u}}_k,{\mathbf{w}}_k)$$

具体形式：

$$\begin{array}{rl}{p}_{x,k+1}& =p_{x,k}+v_{x,k}\cdot \Delta t\\ p_{y,k+1}& =p_{y,k}+v_{y,k}\cdot \Delta t\\ v_{x,k+1}& =v_{x,k}+(f_{x,k}-b_{ax,k})\cos {\psi }_k-(f_{y,k}-b_{ay,k})\sin {\psi }_k\cdot \Delta t\\ v_{y,k+1}& =v_{y,k}+(f_{x,k}-b_{ax,k})\sin {\psi }_k+(f_{y,k}-b_{ay,k})\cos {\psi }_k\cdot \Delta t\\ {\psi }_{k+1}& ={\psi }_k+({\omega }_k-b_{g,k})\cdot \Delta t\\ b_{g,k+1}& =b_{g,k}\\ b_{ax,k+1}& =b_{ax,k}\\ b_{ay,k+1}& =b_{ay,k}\end{array}$$

其中：

• $f_{x,k},f_{y,k}$：IMU测量的比力 (m/s²)
• ${\omega }_k$：IMU测量的角速度 (rad/s)
• $\Delta t$：采样时间间隔

状态转移雅可比矩阵

EKF需要计算状态转移函数的雅可比矩阵 $\mathbf{F}$：

$$\mathbf{F}=\frac{\partial f}{\partial \mathbf{x}}{|}_{{\mathbf{x}}_{k|k-1}}$$

主要的非零元素包括：

$$\begin{array}{rl}{F}_{1,3}& =F_{2,4}=\Delta t\\ F_{3,5}& =-(f_x-b_{ax})\sin \psi -(f_y-b_{ay})\cos \psi \cdot \Delta t\\ F_{4,5}& =(f_x-b_{ax})\cos \psi -(f_y-b_{ay})\sin \psi \cdot \Delta t\\ F_{3,7}& =F_{4,8}=-\cos \psi \cdot \Delta t\\ F_{3,8}& =F_{4,7}=\sin \psi \cdot \Delta t\\ F_{5,6}& =-\Delta t\end{array}$$

观测方程

观测方程为线性的：

$${\mathbf{z}}_k=\mathbf{H}{\mathbf{x}}_k+{\mathbf{v}}_k$$

观测雅可比矩阵 $\mathbf{H}$ 为：

$$\mathbf{H}=\left[\begin{array}{cccccccc}1& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right]$$

噪声模型

过程噪声协方差矩阵Q：

$$\mathbf{Q}=\text{diag}\left[\begin{array}{c}({\sigma }_a\Delta t^2{)}^2{\mathbf{I}}_{2\times 2}\\ ({\sigma }_a\Delta t{)}^2{\mathbf{I}}_{2\times 2}\\ ({\sigma }_{\omega }\Delta t{)}^2\\ ({\sigma }_{bg}\Delta t{)}^2\\ ({\sigma }_{ba}\Delta t{)}^2{\mathbf{I}}_{2\times 2}\end{array}\right]$$

其中：

• ${\sigma }_a=0.01$ m/s²：加速度计噪声标准差
• ${\sigma }_{\omega }=0.1°$ = 0.0017 rad/s：陀螺仪噪声标准差
• ${\sigma }_{bg}=0.01°$ = 0.00017 rad/s：陀螺仪偏差噪声标准差
• ${\sigma }_{ba}=0.001$ m/s²：加速度计偏差噪声标准差

观测噪声协方差矩阵R：

$$\mathbf{R}={\sigma }_{gnss}^2{\mathbf{I}}_{2\times 2}$$

其中 ${\sigma }_{gnss}=3$ m：GNSS位置观测噪声标准差

性能评估

系统通过以下指标评估滤波性能：

1. 位置均方根误差（RMSE）：
   $${\text{RMSE}}_{pos}=\sqrt{\frac{1}{N}\sum _{k=1}^N[(p_{x,k}^{est}-p_{x,k}^{true}{)}^2+(p_{y,k}^{est}-p_{y,k}^{true}{)}^2]}$$

2. 速度均方根误差（RMSE）：
   $${\text{RMSE}}_{vel}=\sqrt{\frac{1}{N}\sum _{k=1}^N[(v_{x,k}^{est}-v_{x,k}^{true}{)}^2+(v_{y,k}^{est}-v_{y,k}^{true}{)}^2]}$$

算法特点

1. 多传感器融合：有效融合高频IMU数据和低频GNSS观测
2. 偏差估计：实时估计并补偿陀螺仪和加速度计偏差
3. 误差状态建模：采用误差状态方法提高线性化精度
4. 实时性能：适合在线实时导航应用

该EKF组合导航算法在保持计算效率的同时，显著提高了导航精度，特别是在GNSS信号中断期间仍能保持较好的位置估计性能。

运行结果

轨迹对比：

各轴位移与速度曲线对比：

误差对比：

命令行截图：

MATLAB源代码

部分代码如下：

% 二维状态量的EKF例程（有严格的组合导航推导）
% 基于8维误差状态模型：位置、速度、航向、航向角角速度偏差、加速度计偏差
% 基于2维的观测模型：XY两个轴的位置
% 作者：matlabfilter
% 2025-08-27/Ver1clear; 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)%% 过程噪声协方差矩阵Q (8×8)
% 状态顺序：[位置(2), 速度(2), 航向角(1), 航向角角速度偏差(1), 加速度计偏差(2)]
Q = zeros(8, 8);
% 位置噪声（通过速度积分产生）
Q(1:2, 1:2) = eye(2) * (accel_noise_std * dt^2)^2;
% 速度噪声
Q(3:4, 3:4) = eye(2) * (accel_noise_std * dt)^2;
% 姿态噪声
Q(5, 5) = eye(1) * (gyro_noise_std * dt)^2;
% 陀螺偏差噪声
Q(6, 6) = eye(1) * (gyro_bias_std * dt)^2;
% 加速度计偏差噪声
Q(7:8, 7:8) = eye(2) * (accel_bias_std * dt)^2;

代码下载链接：
https://download.csdn.net/download/callmeup/91800451

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