基于EKF的INS/GPS组合导航MATLAB仿真

一、系统模型与参数设置

%% 系统参数定义
global dt;
dt = 0.1; % 时间步长(s)
T = 100;  % 仿真总时长(s)
t = 0:dt:T-dt;

% 状态向量维度定义（位置误差、速度误差、姿态误差、传感器偏差）
state_dim = 15; % [lat_err, lon_err, alt_err, vn_err, ve_err, vd_err, roll_err, pitch_err, yaw_err, gyro_bias_x, gyro_bias_y, gyro_bias_z, accel_bias_x, accel_bias_y, accel_bias_z]

% 初始状态与协方差
x0 = zeros(state_dim,1); % 初始误差状态
P0 = diag([0.1^2, 0.1^2, 0.01^2, 0.01^2, 0.01^2, 0.01^2, 0.001^2, 0.001^2, 0.001^2, 0.0001^2, 0.0001^2, 0.0001^2, 0.0001^2, 0.0001^2, 0.0001^2]);

二、核心算法实现

1. 状态转移方程

function x_pred = state_transition(x, dt)
    % 提取状态变量
    [lat_err, lon_err, alt_err, vn_err, ve_err, vd_err, roll_err, pitch_err, yaw_err, ...
     gyro_bx, gyro_by, gyro_bz, accel_bx, accel_by, accel_bz] = x;
    
    % 地球参数
    g0 = 9.78049; % 重力加速度(m/s²)
    Re = 6378137; % 地球半径(m)
    omega_ie = 7.292115e-5; % 地球自转角速度(rad/s)
    
    % 姿态更新（四元数法）
    q = quat_from_euler([roll_err, pitch_err, yaw_err]);
    C_nb = quat2dcm(q);
    
    % 加速度计测量模型
    f_b = [0; 0; g0] + C_nb' * [accel_bx; accel_by; accel_bz] + [0; 0; -9.81];
    
    % 运动学方程
    vn_pred = vn_err + (f_b(1)/m + g0*sin(yaw_err)) * dt;
    ve_pred = ve_err + (f_b(2)/m - g0*cos(yaw_err)) * dt;
    vd_pred = vd_err + f_b(3)/m * dt;
    
    % 位置更新（NED坐标系）
    lat_pred = lat_err + (vn_pred * sin(yaw_err) + ve_pred * cos(yaw_err)) * dt / Re;
    lon_pred = lon_err + (ve_pred * sin(yaw_err) - vn_pred * cos(yaw_err)) * dt / (Re * cos(lat_err));
    alt_pred = alt_err + vd_pred * dt;
    
    % 误差状态更新
    x_pred = [lat_pred, lon_pred, alt_pred, vn_pred, ve_pred, vd_pred, ...
             roll_err, pitch_err, yaw_err, gyro_bx, gyro_by, gyro_bz, accel_bx, accel_by, accel_bz]';
end

2. 观测方程（GPS位置测量）

function z = observation_model(x, truth)
    % 真实位置与估计位置差异
    dx = truth(1) - x(1);
    dy = truth(2) - x(2);
    dz = truth(3) - x(3);
    z = [dx; dy; dz] + mvnrnd([0;0;0], diag([10^2,10^2,30^2]))';
end

三、EKF滤波实现

%% EKF滤波初始化
x_est = x0;
P_est = P0;

% 过程噪声协方差（INS误差模型）
Q = diag([0.01^2, 0.01^2, 0.001^2, 0.001^2, 0.001^2, 0.001^2, 0.0001^2, 0.0001^2, 0.0001^2, 0.00001^2, 0.00001^2, 0.00001^2, 0.00001^2, 0.00001^2, 0.00001^2]);

% 观测噪声协方差（GPS测量噪声）
R = diag([10^2, 10^2, 30^2]);

%% 仿真循环
truth = generate_ground_truth(T); % 生成真实轨迹
[ins_data, gps_data] = simulate_sensors(truth, Q, R); % 生成带噪声的传感器数据

est_states = zeros(size(truth));
for k = 1:T/dt
    % 预测步骤
    x_pred = state_transition(x_est, dt);
    P_pred = F * P_est * F' + Q; % F为状态转移矩阵
    
    % 更新步骤
    z = observation_model(x_pred, truth(:,k));
    K = P_pred * H' / (H * P_pred * H' + R);
    x_est = x_pred + K * (z - H * x_pred);
    P_est = (eye(size(P_pred)) - K*H) * P_pred;
    
    est_states(:,k) = x_est;
end

四、辅助函数实现

1. 真实轨迹生成

function truth = generate_ground_truth(T)
    % 生成螺旋上升轨迹（示例）
    t = 0:0.1:T;
    truth = zeros(3,length(t));
    for i = 1:length(t)
        truth(1,i) = 0.1*sin(0.1*t(i));
        truth(2,i) = 0.1*cos(0.1*t(i));
        truth(3,i) = 0.05*t(i);
    end
end

2. 传感器噪声模拟

function [ins_data, gps_data] = simulate_sensors(truth, Q, R)
    N = length(truth);
    ins_data = zeros(15,N);
    gps_data = zeros(3,N);
    
    % INS误差模拟
    for i = 1:N
        ins_data(:,i) = truth(:,i) + mvnrnd(zeros(15,1), Q)';
    end
    
    % GPS位置测量模拟
    for i = 1:N
        gps_data(:,i) = truth(:,i) + mvnrnd(zeros(3,1), R)';
    end
end

五、性能评估与可视化

%% 结果分析
figure;
subplot(3,1,1);
plot(t, truth(1,:), 'b', t, est_states(1,:), 'r--');
title('纬度误差对比');
xlabel('时间(s)'); ylabel('误差(m)');

subplot(3,1,2);
plot(t, truth(2,:), 'b', t, est_states(2,:), 'r--');
title('经度误差对比');

subplot(3,1,3);
plot(t, truth(3,:), 'b', t, est_states(3,:), 'r--');
title('高度误差对比');

%% 误差统计
mae_lat = mean(abs(est_states(1,:) - truth(1,:)));
rmse_lat = sqrt(mean((est_states(1,:) - truth(1,:)).^2));
disp(['纬度MAE: ', num2str(mae_lat), ' m | RMSE: ', num2str(rmse_lat)]);

参考代码 INS/GPS组合导航仿真代码 www.youwenfan.com/contentsfa/59875.html

六、关键改进方向

1. 多传感器融合

   • 添加地磁传感器数据融合（需扩展状态向量）
   • 实现多普勒测速仪(DVL)辅助定位

2. 动态模型优化

   • 引入车辆运动学约束（如自行车模型）
   • 考虑地球曲率与重力异常修正

3. 实时性优化

   • 采用平方根卡尔曼滤波降低计算量
   • 使用GPU加速大规模矩阵运算