基于导航数据的扩展卡尔曼滤波（EKF）MATLAB 实现
- 非线性运动模型（IMU 预测）
- 线性/非线性观测模型（GPS / 里程计）
- 状态协方差更新
- 时间戳对齐
- 可直接替换为你自己的导航数据
一、EKF 在导航中的典型应用场景
| 场景 | 状态量 | 观测量 |
|---|---|---|
| 车载导航 | 位置、速度、姿态、陀螺零偏、加计零偏 | GPS 位置、车速、航向 |
| 无人机 | 位置、速度、姿态、角速度偏置 | GPS、气压计、磁力计 |
| 行人导航 | 位置、速度、航向 | WiFi、PDR、气压 |
| 船舶/水下 | 位置、速度、姿态 | GPS/DVL/USBL |
下面以车载 2D 导航（北东地坐标系）为例，这是最常用、最稳定的入门级导航 EKF。
二、导航 EKF 状态定义（核心）
状态向量（10 维）
x=pnpevnveψbgbasgsa∇T\mathbf{x} = \begin{bmatrix} p_n & p_e & v_n & v_e & \psi & b_g & b_a & s_g & s_a & \nabla \end{bmatrix}^Tx=pnpevnveψbgbasgsa∇T
| 变量 | 含义 | 单位 |
|---|---|---|
| (pn,pep_n, p_epn,pe) | 北向、东向位置 | m |
| (vn,vev_n, v_evn,ve) | 北向、东向速度 | m/s |
| (ψ\psiψ) | 航向角（偏航） | rad |
| (bgb_gbg) | 陀螺零偏 | rad/s |
| (bab_aba) | 加计零偏 | m/s² |
| (sgs_gsg) | 陀螺刻度因数 | - |
| (sas_asa) | 加计刻度因数 | - |
| (∇\nabla∇) | 加计常值偏置 | m/s² |
工程经验：
- 初学/课程设计：只用位置+速度+姿态（6维）
- 工程落地：必须加零偏和刻度因数
三、EKF 预测步（IMU 驱动）
1、状态转移方程（非线性）
matlab
%% ekf_predict.m
function [x_pred, P_pred] = ekf_predict(x, P, imu, dt, Q)
% x: 状态向量
% P: 协方差矩阵
% imu: [gyro_z, acc_x, acc_y] (车体坐标系)
% dt: 时间间隔
% Q: 过程噪声协方差
% 解算状态
pn = x(1); pe = x(2);
vn = x(3); ve = x(4);
psi = x(5);
bg = x(6); ba = x(7);
sg = x(8); sa = x(9);
nabla = x(10);
% IMU 测量值（补偿误差）
gyro_z = sg * imu(1) + bg; % 补偿刻度因数和零偏
acc_x = sa * imu(2) + ba + nabla; % 补偿刻度因数、零偏和常值偏置
acc_y = sa * imu(3) + ba + nabla;
% 状态更新（2D 平面运动）
pn_pred = pn + vn*dt;
pe_pred = pe + ve*dt;
vn_pred = vn + (acc_x*cos(psi) - acc_y*sin(psi))*dt;
ve_pred = ve + (acc_x*sin(psi) + acc_y*cos(psi))*dt;
psi_pred = psi + gyro_z*dt;
% 零偏和刻度因数缓慢变化（随机游走）
bg_pred = bg;
ba_pred = ba;
sg_pred = sg;
sa_pred = sa;
nabla_pred = nabla;
x_pred = [pn_pred; pe_pred; vn_pred; ve_pred; psi_pred;
bg_pred; ba_pred; sg_pred; sa_pred; nabla_pred];
% ---------- 雅可比矩阵 F ----------
F = eye(10);
% 位置对速度的偏导
F(1,3) = dt; F(2,4) = dt;
% 速度对姿态、加速度的偏导
F(3,5) = (-acc_x*sin(psi) - acc_y*cos(psi))*dt;
F(4,5) = ( acc_x*cos(psi) - acc_y*sin(psi))*dt;
% 速度对加速度的偏导（含刻度因数）
F(3,9) = imu(2)*cos(psi)*dt; % ∂vn/∂sa
F(3,10)= cos(psi)*dt; % ∂vn/∂∇
F(4,9) = imu(2)*sin(psi)*dt; % ∂ve/∂sa
F(4,10)= sin(psi)*dt; % ∂ve/∂∇
% 姿态对陀螺的偏导
F(5,8) = imu(1)*dt; % ∂psi/∂sg
F(5,6) = dt; % ∂psi/∂bg
% 零偏和刻度因数的随机游走
F(6,6) = 1; F(7,7) = 1;
F(8,8) = 1; F(9,9) = 1; F(10,10) = 1;
% ---------- 预测协方差 ----------
P_pred = F * P * F' + Q;
end四、EKF 更新步（GPS / 里程计）
1、位置观测（GPS）
matlab
%% ekf_update_position.m
function [x_upd, P_upd] = ekf_update_position(x_pred, P_pred, z_pos, R_pos)
% z_pos: [pn_gps; pe_gps]
% R_pos: GPS 位置噪声协方差
% 观测矩阵 H (位置直接观测)
H = zeros(2,10);
H(1,1) = 1; % ∂pn/∂pn
H(2,2) = 1; % ∂pe/∂pe
% 观测残差
y = z_pos - H*x_pred;
% 卡尔曼增益
S = H*P_pred*H' + R_pos;
K = P_pred*H'/S;
% 状态更新
x_upd = x_pred + K*y;
% 协方差更新（Joseph 形式，数值更稳定）
I_KH = eye(10) - K*H;
P_upd = I_KH*P_pred*I_KH' + K*R_pos*K';
% 强制对称
P_upd = (P_upd + P_upd')/2;
end2、速度观测（里程计 / 轮速）
matlab
%% ekf_update_velocity.m
function [x_upd, P_upd] = ekf_update_velocity(x_pred, P_pred, z_vel, R_vel)
% z_vel: [vn_odo; ve_odo]
% R_vel: 速度噪声协方差
H = zeros(2,10);
H(1,3) = 1; % ∂vn/∂vn
H(2,4) = 1; % ∂ve/∂ve
y = z_vel - H*x_pred;
S = H*P_pred*H' + R_vel;
K = P_pred*H'/S;
x_upd = x_pred + K*y;
I_KH = eye(10) - K*H;
P_upd = I_KH*P_pred*I_KH' + K*R_vel*K';
P_upd = (P_upd + P_upd')/2;
end五、主程序：导航 EKF 仿真
matlab
%% main_navigation_ekf.m
clear; clc; close all;
%% ========== 仿真参数 ==========
dt = 0.01; % IMU 采样时间 (100Hz)
T = 100; % 总时长 (s)
steps = T/dt;
% GPS 采样率 (10Hz)
gps_dt = 0.1;
gps_steps = gps_dt/dt;
%% ========== 真实轨迹 ==========
% 匀速圆周运动
R = 50; omega = 0.1;
true_pos = zeros(steps,2);
true_vel = zeros(steps,2);
true_psi = zeros(steps,1);
for k = 1:steps
t = k*dt;
true_pos(k,1) = R*sin(omega*t); % 北向
true_pos(k,2) = R*(1-cos(omega*t)); % 东向
true_vel(k,1) = R*omega*cos(omega*t);
true_vel(k,2) = R*omega*sin(omega*t);
true_psi(k) = atan2(true_vel(k,2), true_vel(k,1));
end
%% ========== IMU 数据仿真 ==========
imu_gyro = zeros(steps,1); % 陀螺 z 轴
imu_acc = zeros(steps,2); % 加速度 x,y
for k = 1:steps
% 真实角速度
imu_gyro(k) = omega;
% 真实加速度（向心加速度）
acc_mag = R*omega^2;
imu_acc(k,1) = acc_mag*cos(true_psi(k));
imu_acc(k,2) = acc_mag*sin(true_psi(k));
end
% 添加 IMU 噪声
imu_gyro = imu_gyro + 0.01*randn(steps,1); % 陀螺噪声
imu_acc = imu_acc + 0.1*randn(steps,2); % 加速度噪声
%% ========== GPS 数据仿真 ==========
gps_pos = zeros(ceil(steps/gps_steps),2);
for k = 1:gps_steps:steps
idx = ceil(k/gps_steps);
gps_pos(idx,1) = true_pos(k,1) + 1.0*randn; % GPS 位置噪声 1m
gps_pos(idx,2) = true_pos(k,2) + 1.0*randn;
end
%% ========== EKF 初始化 ==========
x = zeros(10,1);
x(1:2) = gps_pos(1,:)'; % 初始位置
x(3:4) = [0; 0]; % 初始速度
x(5) = 0; % 初始航向
x(6:10) = 0.01*randn(5,1); % 初始误差
P = diag([10^2, 10^2, 1^2, 1^2, 0.1^2, ...
0.01^2, 0.1^2, 0.001^2, 0.001^2, 0.01^2]);
% 噪声协方差
Q = diag([0.1^2, 0.1^2, 0.5^2, 0.5^2, 0.01^2, ...
1e-6, 1e-4, 1e-8, 1e-8, 1e-6]); % 过程噪声
R_gps = diag([1^2, 1^2]); % GPS 位置噪声
%% ========== EKF 主循环 ==========
x_est = zeros(10,steps);
P_est = zeros(10,10,steps);
for k = 1:steps
% ---------- 1. 预测步 ----------
imu = [imu_gyro(k), imu_acc(k,1), imu_acc(k,2)]';
[x, P] = ekf_predict(x, P, imu, dt, Q);
% ---------- 2. 更新步（GPS） ----------
if mod(k-1, gps_steps) == 0
gps_idx = ceil(k/gps_steps);
if gps_idx <= size(gps_pos,1)
z_gps = gps_pos(gps_idx,:)';
[x, P] = ekf_update_position(x, P, z_gps, R_gps);
end
end
% 存储结果
x_est(:,k) = x;
P_est(:,:,k) = P;
end
%% ========== 结果可视化 ==========
figure('Color','white','Position',[100 100 1200 800])
% 轨迹对比
subplot(2,2,1)
plot(true_pos(:,1), true_pos(:,2), 'b-', 'LineWidth',1.5)
hold on
plot(x_est(1,:), x_est(2,:), 'r--', 'LineWidth',1.5)
plot(gps_pos(:,1), gps_pos(:,2), 'go', 'MarkerSize',3)
xlabel('North (m)'); ylabel('East (m)')
title('Trajectory Comparison')
legend('True', 'EKF Estimate', 'GPS Measurement')
grid on; axis equal
% 位置误差
subplot(2,2,2)
pos_error = sqrt((x_est(1,:)-true_pos(:,1)).^2 + ...
(x_est(2,:)-true_pos(:,2)).^2);
plot((1:steps)*dt, pos_error, 'b-', 'LineWidth',1.2)
xlabel('Time (s)'); ylabel('Position Error (m)')
title('Position Error')
grid on
% 速度误差
subplot(2,2,3)
vel_error = sqrt((x_est(3,:)-true_vel(:,1)).^2 + ...
(x_est(4,:)-true_vel(:,2)).^2);
plot((1:steps)*dt, vel_error, 'r-', 'LineWidth',1.2)
xlabel('Time (s)'); ylabel('Velocity Error (m/s)')
title('Velocity Error')
grid on
% 航向误差
subplot(2,2,4)
psi_error = wrapToPi(x_est(5,:) - true_psi');
plot((1:steps)*dt, rad2deg(psi_error), 'g-', 'LineWidth',1.2)
xlabel('Time (s)'); ylabel('Heading Error (deg)')
title('Heading Error')
grid on
sgtitle('Navigation EKF Performance', 'FontSize', 14)参考代码 EKF的实现,基于导航数据实现的扩展卡尔曼滤波 www.youwenfan.com/contentcsw/82782.html
六、工程级改进
1、 导航数据预处理
matlab
% 时间戳对齐（关键！）
t_imu = (0:dt:T-dt)';
t_gps = (0:gps_dt:T-gps_dt)';
gps_interp = interp1(t_gps, gps_pos, t_imu, 'linear');
% 异常值剔除（卡方检验）
innovation = z - H*x_pred;
if innovation'*inv(S)*innovation > 9.21 % 99%置信区间
% 拒绝更新
end2、传感器融合扩展
| 传感器 | 观测模型 | 作用 |
|---|---|---|
| 磁力计 | 航向角观测 | 抑制航向漂移 |
| 气压计 | 高度观测 | 3D 导航 |
| 轮速里程计 | 速度观测 | 抑制速度漂移 |
| 视觉/激光 | 位置/特征观测 | 高精度定位 |
3、数值稳定性技巧
matlab
% 1. Joseph 形式更新协方差（已用）
% 2. 协方差对称化
P = (P + P')/2;
% 3. 防止协方差爆炸
P = P + 1e-12*eye(size(P));
% 4. 平方根滤波（SR-EKF）七、与 UKF / PF 的对比
| 滤波器 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|
| EKF | 计算量小、成熟 | 线性化误差 | 大多数导航系统 |
| UKF | 精度高、无需雅可比 | 计算量大 | 强非线性系统 |
| PF | 任意分布 | 粒子退化 | 非高斯噪声 |