EKF组合导航系统：惯性导航与组合导航MATLAB实现
扩展卡尔曼滤波(EKF)是组合导航系统的核心算法，通过融合惯性导航系统(INS)和其他导航传感器(如GPS、磁力计等)的数据，实现高精度导航定位。以下是完整的MATLAB实现方案。
一、导航系统基础
1. 坐标系定义
- 地心地固坐标系(ECEF)：原点在地心，Z轴指向北极
- 当地地理坐标系(NED)：原点在载体位置，X轴指北，Y轴指东，Z轴向下
- 载体坐标系(Body Frame)：原点在载体质心，X轴向前，Y轴向右，Z轴向下
2. 传感器模型
- 加速度计：测量比力
- 陀螺仪：测量角速度
- GPS：提供位置
、速度 - 磁力计：提供地磁场方向
二、惯性导航系统(INS)机械编排
1. 姿态更新(四元数法)
function q = quat_update(q, gyro, dt)
% 四元数更新
% 输入: q - 当前四元数 [q0, q1, q2, q3]
% gyro - 陀螺仪测量值 [wx, wy, wz] (rad/s)
% dt - 时间步长 (s)
% 输出: q_new - 更新后的四元数
wx = gyro(1); wy = gyro(2); wz = gyro(3);
omega = [0, wx, wy, wz;
-wx, 0, -wz, wy;
-wy, wz, 0, -wx;
-wz, -wy, wx, 0];
q_vec = q(:);
dq = 0.5 * omega * q_vec;
q_new = q_vec + dq * dt;
q_new = q_new / norm(q_new); % 归一化
end
2. 速度更新
function v = velocity_update(v, q, accel, g, dt)
% 速度更新
% 输入: v - 当前速度 [vn, ve, vd] (m/s)
% q - 当前四元数
% accel - 加速度计测量值 [ax, ay, az] (m/s²)
% g - 重力加速度 [0, 0, g] (m/s²)
% dt - 时间步长 (s)
% 输出: v_new - 更新后的速度
% 转换到导航坐标系
Cbn = quat2dcm(q); % 四元数转方向余弦矩阵
f_n = Cbn * accel(:); % 比力在导航系投影
% 速度更新
v_new = v + (f_n + g) * dt;
end
3. 位置更新
function p = position_update(p, v, dt)
% 位置更新
% 输入: p - 当前位置 [lat, lon, alt] (rad, rad, m)
% v - 当前速度 [vn, ve, vd] (m/s)
% dt - 时间步长 (s)
% 输出: p_new - 更新后的位置
R = 6378137; % 地球半径 (m)
lat = p(1); lon = p(2); alt = p(3);
% 计算位置变化
dlat = v(1) * dt / R;
dlon = v(2) * dt / (R * cos(lat));
dalt = -v(3) * dt; % 注意: 导航系Z轴向下
p_new = [lat + dlat; lon + dlon; alt + dalt];
end
三、扩展卡尔曼滤波(EKF)实现
1. 状态向量定义
% 状态向量: [位置误差, 速度误差, 姿态误差, 加速度计偏置, 陀螺仪偏置]
% 15维: [δφ, δλ, δh, δvn, δve, δvd, δφ, δθ, δψ, ba_x, ba_y, ba_z, bg_x, bg_y, bg_z]
2. EKF初始化
function [x, P] = init_ekf(initial_error, initial_covariance)
% 初始化EKF状态和协方差矩阵
% 输入: initial_error - 初始状态误差估计
% initial_covariance - 初始协方差矩阵
% 输出: x - 初始状态向量
% P - 初始协方差矩阵
x = initial_error(:); % 状态向量
P = initial_covariance; % 协方差矩阵
end
3. 状态预测
function [x_pred, P_pred] = predict_state(x, P, imu, dt, Q)
% EKF预测步骤
% 输入: x - 当前状态向量
% P - 当前协方差矩阵
% imu - IMU数据 [accel, gyro]
% dt - 时间步长
% Q - 过程噪声协方差矩阵
% 输出: x_pred - 预测状态
% P_pred - 预测协方差
% 1. 状态转移矩阵F (简化模型)
F = eye(15);
% 设置姿态误差传播项
F(7:9, 10:12) = -eye(3)*dt; % 姿态误差与加速度计偏置关系
F(7:9, 13:15) = eye(3)*dt; % 姿态误差与陀螺仪偏置关系
% 2. 控制输入矩阵G (IMU噪声影响)
G = zeros(15, 12);
G(4:6, 1:3) = eye(3)*dt; % 速度误差受加速度计噪声影响
G(7:9, 4:6) = eye(3)*dt; % 姿态误差受陀螺仪噪声影响
G(10:12, 7:9) = eye(3); % 加速度计偏置随机游走
G(13:15, 10:12) = eye(3); % 陀螺仪偏置随机游走
% 3. 状态预测
x_pred = F * x; % 线性模型简化
% 4. 协方差预测
P_pred = F * P * F' + G * Q * G';
end
4. 量测更新
function [x_updated, P_updated] = update_state(x_pred, P_pred, z, R, H)
% EKF更新步骤
% 输入: x_pred - 预测状态
% P_pred - 预测协方差
% z - 量测向量
% R - 量测噪声协方差
% H - 量测矩阵
% 输出: x_updated - 更新后状态
% P_updated - 更新后协方差
% 1. 计算卡尔曼增益
K = P_pred * H' / (H * P_pred * H' + R);
% 2. 状态更新
x_updated = x_pred + K * (z - H * x_pred);
% 3. 协方差更新
P_updated = (eye(15) - K * H) * P_pred;
end
四、完整组合导航系统实现
1. 主程序框架
function integrated_navigation()
% 组合导航主程序
% 参数设置
dt = 0.01; % 采样时间 (s)
total_time = 100; % 总仿真时间 (s)
steps = total_time / dt;
% 初始化
[imu_data, gps_data, true_trajectory] = generate_test_data(dt, steps);
[x, P] = init_ekf(zeros(15,1), diag([0.1*ones(1,3), 0.1*ones(1,3), 0.01*ones(1,3), 0.01*ones(1,3), 0.001*ones(1,3)]));
Q = diag([0.01*ones(1,3), 0.01*ones(1,3), 0.001*ones(1,3), 0.001*ones(1,3), 1e-6*ones(1,3)]); % 过程噪声
R_gps = diag([1, 1, 1, 0.1, 0.1, 0.1]); % GPS位置速度噪声
% 存储结果
estimated_trajectory = zeros(steps, 6); % [lat, lon, alt, vn, ve, vd]
ins_trajectory = zeros(steps, 6);
% 主循环
for k = 1:steps
% 1. 读取IMU数据
accel = imu_data(k, 1:3);
gyro = imu_data(k, 4:6);
% 2. INS机械编排
[q, v, p] = ins_mechanization(q, v, p, accel, gyro, dt);
ins_trajectory(k, :) = [p(1), p(2), p(3), v(1), v(2), v(3)];
% 3. EKF预测
[x_pred, P_pred] = predict_state(x, P, [accel; gyro], dt, Q);
% 4. GPS更新 (每1秒更新一次)
if mod(k, 100) == 0
gps_pos = gps_data(k, 1:3);
gps_vel = gps_data(k, 4:6);
z = [gps_pos; gps_vel] - [p; v]; % 量测残差
% 量测矩阵H (GPS位置速度)
H = [eye(3), zeros(3,3), eye(3), zeros(3,6);
zeros(3,3), eye(3), zeros(3,3), zeros(3,6)];
% EKF更新
[x, P] = update_state(x_pred, P_pred, z, R_gps, H);
else
x = x_pred;
P = P_pred;
end
% 5. 状态反馈校正
p = p + x(1:3);
v = v + x(4:6);
q = correct_quaternion(q, x(7:9)); % 小角度近似校正姿态
% 6. 存储结果
estimated_trajectory(k, :) = [p(1), p(2), p(3), v(1), v(2), v(3)];
end
% 结果可视化
plot_results(true_trajectory, ins_trajectory, estimated_trajectory);
end
2. INS机械编排封装
function [q_new, v_new, p_new] = ins_mechanization(q, v, p, accel, gyro, dt)
% INS机械编排封装函数
% 1. 姿态更新
q_new = quat_update(q, gyro, dt);
% 2. 速度更新
g = [0; 0; 9.81]; % 重力加速度 (m/s²)
v_new = velocity_update(v, q_new, accel, g, dt);
% 3. 位置更新
p_new = position_update(p, v_new, dt);
end
3. 四元数辅助函数
function Cbn = quat2dcm(q)
% 四元数转方向余弦矩阵
q0 = q(1); q1 = q(2); q2 = q(3); q3 = q(4);
Cbn = [1-2*(q2^2+q3^2), 2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2);
2*(q1*q2+q0*q3), 1-2*(q1^2+q3^2), 2*(q2*q3-q0*q1);
2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), 1-2*(q1^2+q2^2)];
end
function q = correct_quaternion(q, delta_theta)
% 小角度姿态校正
dq = [1; 0.5*delta_theta];
q_new = quat_multiply(q, dq);
q = q_new / norm(q_new);
end
function q_out = quat_multiply(q1, q2)
% 四元数乘法
q0 = q1(1)*q2(1) - q1(2)*q2(2) - q1(3)*q2(3) - q1(4)*q2(4);
q1_out = q1(1)*q2(2) + q1(2)*q2(1) + q1(3)*q2(4) - q1(4)*q2(3);
q2_out = q1(1)*q2(3) - q1(2)*q2(4) + q1(3)*q2(1) + q1(4)*q2(2);
q3_out = q1(1)*q2(4) + q1(2)*q2(3) - q1(3)*q2(2) + q1(4)*q2(1);
q_out = [q0; q1_out; q2_out; q3_out];
end
五、测试数据生成
function [imu_data, gps_data, true_trajectory] = generate_test_data(dt, steps)
% 生成测试数据
% 真实轨迹 (圆周运动)
t = (0:dt:(steps-1)*dt)';
radius = 100; % 半径 (m)
speed = 10; % 速度 (m/s)
omega = speed / radius; % 角速度 (rad/s)
true_pos = zeros(steps, 3);
true_vel = zeros(steps, 3);
true_att = zeros(steps, 3);
for k = 1:steps
theta = omega * t(k);
true_pos(k, :) = [radius*sin(theta), radius*(1-cos(theta)), 0];
true_vel(k, :) = [speed*cos(theta), speed*sin(theta), 0];
true_att(k, :) = [0, 0, theta]; % 航向角
end
% IMU数据 (添加噪声和偏置)
accel_bias = [0.1, -0.05, 0.2]; % 加速度计偏置 (m/s²)
gyro_bias = [0.01, -0.02, 0.03]; % 陀螺仪偏置 (rad/s)
imu_data = zeros(steps, 6);
for k = 1:steps
% 理想加速度 (圆周运动向心加速度 + 重力)
a_centripetal = [-speed^2/radius*sin(true_att(k,3)), speed^2/radius*cos(true_att(k,3)), 0];
a_total = a_centripetal + [0, 0, 9.81]; % 加上重力
% 添加噪声
accel_noise = 0.05 * randn(1,3);
gyro_noise = 0.005 * randn(1,3);
imu_data(k, 1:3) = a_total + accel_bias + accel_noise;
imu_data(k, 4:6) = [0, 0, omega] + gyro_bias + gyro_noise; % 角速度
end
% GPS数据 (添加噪声)
gps_data = zeros(steps, 6);
gps_pos_noise = 1.0; % 位置噪声 (m)
gps_vel_noise = 0.1; % 速度噪声 (m/s)
for k = 1:steps
gps_data(k, 1:3) = true_pos(k, :) + gps_pos_noise * randn(1,3);
gps_data(k, 4:6) = true_vel(k, :) + gps_vel_noise * randn(1,3);
end
true_trajectory = [true_pos, true_vel];
end
六、结果可视化
function plot_results(true_traj, ins_traj, ekf_traj)
% 结果可视化
figure('Name', '导航轨迹对比', 'Position', [100, 100, 1200, 800]);
% XY平面轨迹
subplot(2,2,1);
plot(true_traj(:,1), true_traj(:,2), 'g-', 'LineWidth', 2); hold on;
plot(ins_traj(:,1), ins_traj(:,2), 'r--', 'LineWidth', 1.5);
plot(ekf_traj(:,1), ekf_traj(:,2), 'b-.', 'LineWidth', 1.5);
xlabel('X位置 (m)'); ylabel('Y位置 (m)');
title('XY平面轨迹');
legend('真实轨迹', '纯INS', 'EKF组合导航');
grid on;
% 位置误差
subplot(2,2,2);
ins_error = sqrt(sum((ins_traj(:,1:3) - true_traj(:,1:3)).^2, 2));
ekf_error = sqrt(sum((ekf_traj(:,1:3) - true_traj(:,1:3)).^2, 2));
plot(ins_error, 'r-', 'LineWidth', 1.5); hold on;
plot(ekf_error, 'b-', 'LineWidth', 1.5);
xlabel('时间步'); ylabel('位置误差 (m)');
title('位置误差');
legend('纯INS', 'EKF组合导航');
grid on;
% 速度对比
subplot(2,2,3);
plot(true_traj(:,4), 'g-', 'LineWidth', 2); hold on;
plot(ins_traj(:,4), 'r--', 'LineWidth', 1.5);
plot(ekf_traj(:,4), 'b-.', 'LineWidth', 1.5);
xlabel('时间步'); ylabel('X速度 (m/s)');
title('X方向速度');
legend('真实值', '纯INS', 'EKF组合导航');
grid on;
% 3D轨迹
subplot(2,2,4);
plot3(true_traj(:,1), true_traj(:,2), true_traj(:,3), 'g-', 'LineWidth', 2); hold on;
plot3(ins_traj(:,1), ins_traj(:,2), ins_traj(:,3), 'r--', 'LineWidth', 1.5);
plot3(ekf_traj(:,1), ekf_traj(:,2), ekf_traj(:,3), 'b-.', 'LineWidth', 1.5);
xlabel('X位置 (m)'); ylabel('Y位置 (m)'); zlabel('Z位置 (m)');
title('3D轨迹');
legend('真实轨迹', '纯INS', 'EKF组合导航');
grid on;
view(3);
end
参考代码 ekf组合导航，惯性导航及其组合导航matlab代码 www.youwenfan.com/contentcnt/63300.html
七、系统优化与扩展
1. 自适应滤波
function [x_updated, P_updated] = adaptive_update(x_pred, P_pred, z, R_base, H, innovation)
% 自适应EKF更新
% 根据新息调整量测噪声
% 计算新息协方差
S = H * P_pred * H' + R_base;
actual_S = innovation * innovation';
% 调整量测噪声
R_adapted = R_base * (actual_S / S);
% 标准EKF更新
K = P_pred * H' / (H * P_pred * H' + R_adapted);
x_updated = x_pred + K * (z - H * x_pred);
P_updated = (eye(15) - K * H) * P_pred;
end
2. 多传感器融合
function [x_updated, P_updated] = multi_sensor_fusion(x_pred, P_pred, sensors, measurements)
% 多传感器融合
for i = 1:length(sensors)
sensor = sensors{i};
z = measurements{i};
switch sensor
case 'GPS'
R = diag([1, 1, 1, 0.1, 0.1, 0.1]);
H = [eye(3), zeros(3,3), eye(3), zeros(3,6);
zeros(3,3), eye(3), zeros(3,3), zeros(3,6)];
case 'MAG'
R = diag([0.1, 0.1, 0.1]);
H = [zeros(3,6), eye(3), zeros(3,6)]; % 仅姿态误差
case 'BARO'
R = diag([0.5]);
H = [eye(1), zeros(1,2), zeros(1,3), zeros(1,3), zeros(1,6)]; % 仅高度
end
[x_pred, P_pred] = update_state(x_pred, P_pred, z, R, H);
end
x_updated = x_pred;
P_updated = P_pred;
end
3. 零速检测与更新
function [x_updated, P_updated] = zero_velocity_update(x_pred, P_pred, v_ins, threshold)
% 零速检测与更新
if norm(v_ins) < threshold
% 检测到零速
z = zeros(3,1); % 速度应为零
H = [zeros(3,3), eye(3), zeros(3,3), zeros(3,6)]; % 速度误差观测
R = diag([0.01, 0.01, 0.01]); % 零速假设噪声
[x_updated, P_updated] = update_state(x_pred, P_pred, z, R, H);
else
x_updated = x_pred;
P_updated = P_pred;
end
end
八、实际部署注意事项
- 时间同步：确保IMU、GPS等传感器数据的时间戳同步
- 初始对准：系统启动时需要进行初始姿态对准
- 杆臂补偿：考虑传感器与载体中心之间的物理偏移
- 地球模型：使用更精确的地球重力模型和地球自转参数
- 异常处理：设计传感器故障检测和容错机制
九、总结
本实现提供了完整的EKF组合导航系统框架，包括：
- INS机械编排（姿态、速度、位置更新）
- EKF核心算法（预测与更新）
- 多传感器融合（GPS、磁力计等）
- 测试数据生成与结果可视化
- 系统优化技术（自适应滤波、零速更新等）
