程序详解
概述
本代码实现基于扩展卡尔曼滤波器（EKF）的二维组合导航系统，融合IMU（惯性测量单元）和GNSS（全球导航卫星系统）数据，实现精确的位置和速度估计。该系统采用8维误差状态模型，在圆形轨迹上进行仿真验证。
系统架构
状态向量定义
系统采用8维状态向量：
x = [ p x p y v x v y ψ b g b a x b a y ] \mathbf{x} = \begin{bmatrix} p_x \\ p_y \\ v_x \\ v_y \\ \psi \\ b_g \\ b_{ax} \\ b_{ay} \end{bmatrix} x= pxpyvxvyψbgbaxbay
其中：
- p x , p y p_x, p_y px,py：X、Y方向位置 (m)
- v x , v y v_x, v_y vx,vy：X、Y方向速度 (m/s)
- ψ \psi ψ：航向角 (rad)
- b g b_g bg：陀螺仪偏差 (rad/s)
- b a x , b a y b_{ax}, b_{ay} bax,bay：X、Y方向加速度计偏差 (m/s²)
观测向量定义
观测向量为2维GNSS位置观测：
z = [ p x p y ] \mathbf{z} = \begin{bmatrix} p_x \\ p_y \end{bmatrix} z=[pxpy]
核心数学模型
状态转移方程
系统的非线性状态转移方程为：
x k + 1 = f ( x k , u k , w k ) \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k, \mathbf{w}_k) xk+1=f(xk,uk,wk)
具体形式：
p x , k + 1 = p x , k + v x , k ⋅ Δ t p y , k + 1 = p y , k + v y , k ⋅ Δ t v x , k + 1 = v x , k + ( f x , k − b a x , k ) cos ψ k − ( f y , k − b a y , k ) sin ψ k ⋅ Δ t v y , k + 1 = v y , k + ( f x , k − b a x , k ) sin ψ k + ( f y , k − b a y , k ) cos ψ k ⋅ Δ t ψ k + 1 = ψ k + ( ω k − b g , k ) ⋅ Δ t b g , k + 1 = b g , k b a x , k + 1 = b a x , k b a y , k + 1 = b a y , k \begin{aligned} 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{aligned} px,k+1py,k+1vx,k+1vy,k+1ψk+1bg,k+1bax,k+1bay,k+1=px,k+vx,k⋅Δt=py,k+vy,k⋅Δt=vx,k+(fx,k−bax,k)cosψk−(fy,k−bay,k)sinψk⋅Δt=vy,k+(fx,k−bax,k)sinψk+(fy,k−bay,k)cosψk⋅Δt=ψk+(ωk−bg,k)⋅Δt=bg,k=bax,k=bay,k
其中：
- f x , k , f y , k f_{x,k}, f_{y,k} fx,k,fy,k：IMU测量的比力 (m/s²)
- ω k \omega_k ωk：IMU测量的角速度 (rad/s)
- Δ t \Delta t Δt：采样时间间隔
状态转移雅可比矩阵
EKF需要计算状态转移函数的雅可比矩阵 F \mathbf{F} F：
F = ∂ f ∂ x ∣ x k ∣ k − 1 \mathbf{F} = \frac{\partial f}{\partial \mathbf{x}} \bigg|_{\mathbf{x}_{k|k-1}} F=∂x∂f xk∣k−1
主要的非零元素包括：
F 1 , 3 = F 2 , 4 = Δ t F 3 , 5 = − ( f x − b a x ) sin ψ − ( f y − b a y ) cos ψ ⋅ Δ t F 4 , 5 = ( f x − b a x ) cos ψ − ( f y − b a y ) sin ψ ⋅ Δ t F 3 , 7 = F 4 , 8 = − cos ψ ⋅ Δ t F 3 , 8 = F 4 , 7 = sin ψ ⋅ Δ t F 5 , 6 = − Δ t \begin{aligned} 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{aligned} F1,3F3,5F4,5F3,7F3,8F5,6=F2,4=Δt=−(fx−bax)sinψ−(fy−bay)cosψ⋅Δt=(fx−bax)cosψ−(fy−bay)sinψ⋅Δt=F4,8=−cosψ⋅Δt=F4,7=sinψ⋅Δt=−Δt
观测方程
观测方程为线性的：
z k = H x k + v k \mathbf{z}_k = \mathbf{H}\mathbf{x}_k + \mathbf{v}_k zk=Hxk+vk
观测雅可比矩阵 H \mathbf{H} H 为：
H = [ 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 ] \mathbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix} H=[1001000000000000]
噪声模型
过程噪声协方差矩阵Q：
Q = diag [ ( σ a Δ t 2 ) 2 I 2 × 2 ( σ a Δ t ) 2 I 2 × 2 ( σ ω Δ t ) 2 ( σ b g Δ t ) 2 ( σ b a Δ t ) 2 I 2 × 2 ] \mathbf{Q} = \text{diag}\begin{bmatrix} (\sigma_{a} \Delta t^2)^2 \mathbf{I}_{2×2} \\ (\sigma_{a} \Delta t)^2 \mathbf{I}_{2×2} \\ (\sigma_{\omega} \Delta t)^2 \\ (\sigma_{bg} \Delta t)^2 \\ (\sigma_{ba} \Delta t)^2 \mathbf{I}_{2×2} \end{bmatrix} Q=diag (σaΔt2)2I2×2(σaΔt)2I2×2(σωΔt)2(σbgΔt)2(σbaΔt)2I2×2
其中：
- σ a = 0.01 \sigma_a = 0.01 σa=0.01 m/s²：加速度计噪声标准差
- σ ω = 0.1 ° \sigma_{\omega} = 0.1° σω=0.1° = 0.0017 rad/s：陀螺仪噪声标准差
- σ b g = 0.01 ° \sigma_{bg} = 0.01° σbg=0.01° = 0.00017 rad/s：陀螺仪偏差噪声标准差
- σ b a = 0.001 \sigma_{ba} = 0.001 σba=0.001 m/s²：加速度计偏差噪声标准差
观测噪声协方差矩阵R：
R = σ g n s s 2 I 2 × 2 \mathbf{R} = \sigma_{gnss}^2 \mathbf{I}_{2×2} R=σgnss2I2×2
其中 σ g n s s = 3 \sigma_{gnss} = 3 σgnss=3 m：GNSS位置观测噪声标准差
性能评估
系统通过以下指标评估滤波性能：
位置均方根误差（RMSE）：
RMSE p o s = 1 N ∑ k = 1 N [ ( p x , k e s t − p x , k t r u e ) 2 + ( p y , k e s t − p y , k t r u e ) 2 ] \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]} RMSEpos=N1k=1∑N[(px,kest−px,ktrue)2+(py,kest−py,ktrue)2]速度均方根误差（RMSE）：
RMSE v e l = 1 N ∑ k = 1 N [ ( v x , k e s t − v x , k t r u e ) 2 + ( v y , k e s t − v y , k t r u e ) 2 ] \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]} RMSEvel=N1k=1∑N[(vx,kest−vx,ktrue)2+(vy,kest−vy,ktrue)2]
算法特点
- 多传感器融合：有效融合高频IMU数据和低频GNSS观测
- 偏差估计：实时估计并补偿陀螺仪和加速度计偏差
- 误差状态建模：采用误差状态方法提高线性化精度
- 实时性能：适合在线实时导航应用
该EKF组合导航算法在保持计算效率的同时，显著提高了导航精度，特别是在GNSS信号中断期间仍能保持较好的位置估计性能。
运行结果
轨迹对比：
各轴位移与速度曲线对比：
误差对比：
命令行截图：
MATLAB源代码
部分代码如下：
% 二维状态量的EKF例程（有严格的组合导航推导）
% 基于8维误差状态模型：位置、速度、航向、航向角角速度偏差、加速度计偏差
% 基于2维的观测模型：XY两个轴的位置
% 作者：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)
%% 过程噪声协方差矩阵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
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者