✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。
🍎 往期回顾关注个人主页：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。
🔥 内容介绍
基于左不变扩展卡尔曼滤波（Invariant Extended Kalman Filter, InEKF）的多传感器融合定位系统，核心是针对刚体运动的李群流形特性设计滤波框架，将 IMU（角速度、角加速度、线加速度）的高频率运动测量与 GPS 的低频率位置 / 速度全局观测进行融合，解决传统 EKF 在姿态表示中因李群非欧氏空间导致的误差耦合、雅可比矩阵复杂及滤波发散问题。
本文从核心理论基础、系统模型构建（IMU 运动模型 + GPS 观测模型）、InEKF 滤波核心流程、多传感器数据融合实现、MATLAB 工程化代码及性能验证展开，实现无人机 / 移动载体的高精度、高鲁棒性定位，适配 GPS 失锁短时间内的 IMU 纯惯导推算需求。
一、核心理论基础
1.1 刚体运动的李群 / 李代数表示
Image
1.2 InEKF 与传统 EKF 的核心差异
特性
传统 EKF（欧拉角 / 四元数）
左不变 EKF（
SE(3)
李群）
状态空间
欧氏空间（近似）
非欧氏李群
SE(3)
（精确）
误差定义
欧氏空间加性误差
李群左乘乘性误差，线性化在
se(3)
雅可比计算
姿态雅可比复杂，易引入数值误差
利用李群李代数性质，雅可比矩阵简化为常数矩阵
滤波稳定性
误差协方差易非物理膨胀，易发散
左不变性保证协方差物理意义，稳定性大幅提升
计算效率
雅可比实时计算，开销高
雅可比简化，计算量降低，适配高频率 IMU 更新
适用场景
低动态、短时间定位
高动态载体（无人机 / 车辆）、长时间多传感器融合
Image
Image
⛳️ 运行结果
Image
📣 部分代码
clear all; clc;
% IMU data
ms25 = readtable('ms25.csv');
t = table2array(ms25(1:floor(end/2), 1));
t = t/1e6;
mag_x = table2array(ms25(1:floor(end/2), 2));
mag_y = table2array(ms25(1:floor(end/2), 3));
mag_z = table2array(ms25(1:floor(end/2), 4));
accel_x = table2array(ms25(1:floor(end/2), 5));
accel_y = table2array(ms25(1:floor(end/2), 6));
accel_z = table2array(ms25(1:floor(end/2), 7));
rotational_x = table2array(ms25(1:floor(end/2), 8));
rotational_y = table2array(ms25(1:floor(end/2), 9));
rotational_z = table2array(ms25(1:floor(end/2), 10));
delta_t = zeros(size(t));
delta_t(1) = t(1);
IMU_data = struct([]);
for t_temp = 2:length(t)
delta_t(t_temp) = (t(t_temp) - t(t_temp-1));
end
for t_temp = 1:length(t)
IMU_data(t_temp,:) = struct;
end
for t_temp = 1:length(t)
IMU_data(t_temp).Time = t(t_temp);
IMU_data(t_temp).dt = delta_t(t_temp);
IMU_data(t_temp).accelX = accel_x(t_temp);
IMU_data(t_temp).accelY = accel_y(t_temp);
IMU_data(t_temp).accelZ = accel_z(t_temp);
IMU_data(t_temp).omegaX = rotational_x(t_temp);
IMU_data(t_temp).omegaY = rotational_y(t_temp);
IMU_data(t_temp).omegaZ = rotational_z(t_temp);
end
%% GPS data
GPS = readtable('gps.csv');
t2 = table2array(GPS(1:floor(end/2), 1))/1e6;
latitude = table2array(GPS(1:floor(end/2), 4));
longitude = table2array(GPS(1:floor(end/2), 5));
altitude = table2array(GPS(1:floor(end/2), 6));
GPS_data = struct([]);
delta_t_gps = zeros(size(t2));
delta_t_gps(1) = t2(1);
for t_temp = 2:length(t2)
delta_t_gps(t_temp) = (t2(t_temp) - t2(t_temp-1));
end
for t_temp = 1:length(t2)
GPS_data(t_temp,:) = struct;
end
for t_temp = 1:length(t2)
GPS_data(t_temp).Time = t2(t_temp);
GPS_data(t_temp).dt = delta_t_gps(t_temp);
GPS_data(t_temp).X = (latitude(t_temp) - latitude(1)) * 180 / pi * 111139 ;
GPS_data(t_temp).Y = (longitude(t_temp) - longitude(1)) * 180 /pi * 111139;
GPS_data(t_temp).Z = altitude(t_temp) - altitude(1);
end
%%
clc; clear all; close all;
load('filtered_GPS_data.mat');
t = [GPS_data.Time];
filtered_GPS_data = struct([]);
delta_t = zeros(1, size(t,2));
delta_t(1) = 0;
for t_temp = 2:length(GPS_data)
delta_t(t_temp) = t(t_temp) - t(t_temp-1);
end
for t_temp = 1:length(t)
filtered_GPS_data(t_temp,:) = struct;
end
for t_temp = 1:length(GPS_data)
filtered_GPS_data(t_temp).Time = GPS_data(t_temp).Time;
filtered_GPS_data(t_temp).dt = delta_t(t_temp);
filtered_GPS_data(t_temp).X = GPS_data(t_temp).X;
filtered_GPS_data(t_temp).Y = GPS_data(t_temp).Y;
filtered_GPS_data(t_temp).Z = 0;
end
GPSData = [filtered_GPS_data.Time; filtered_GPS_data.dt; filtered_GPS_data.X; filtered_GPS_data.Y; filtered_GPS_data.Z]';
🔗 参考文献
🎈 部分理论引用网络文献，若有侵权联系博主删除
