【滤波跟踪】基于扩展卡尔曼滤波EKF惯性导航系统INS代码，实现融合 IMU、GPS、磁力计、气压计数据的导航解算功能附matlab代码
✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。
🍎 往期回顾关注个人主页：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。
🔥 内容介绍
- 传感器特性
：IMU 可提供姿态、角速度和加速度信息，但存在累积误差。GPS 能提供全局位置和速度信息，不过易受信号遮挡影响。磁力计可辅助测量航向，却受电磁干扰。气压计用于测量高度，会受温度和环境压力波动影响。
- EKF 原理及优势
：EKF 是卡尔曼滤波器针对非线性系统的扩展，通过对非线性系统模型和测量模型进行一阶泰勒展开，将其近似为线性系统，再应用卡尔曼滤波器进行状态估计。其优势在于能处理非线性系统，适用于 INS 这种非线性特性明显的系统，但存在线性化误差，对初始状态和协方差选取敏感，且需计算雅可比矩阵，计算量较大。
- 导航解算过程
：首先建立 INS 的误差状态模型，误差状态向量通常包括姿态误差、速度误差、位置误差、陀螺仪零偏和加速度计零偏等。然后推导其动态方程和观测方程，动态方程描述误差状态的传播规律，观测方程描述观测值与误差状态之间的关系。最后通过 EKF 的预测和更新步骤，对误差状态进行估计和校正，从而实现导航解算。
- 信息融合方式
：多源信息融合算法根据处理层次可分为数据级融合、特征级融合和决策级融合。对于融合 IMU、GPS、磁力计、气压计数据，常采用特征级融合，提取姿态角、速度等特征进行融合，平衡精度与计算效率。也可采用多级融合架构，如先融合陀螺仪、加速度计、磁力计的姿态信息，再分别将加速度计、气压计与 GPS 融合，提升不同维度的导航精度。
- 应用场景及意义
：该技术广泛应用于多旋翼无人机导航等领域，可实现高精度导航，提升导航系统的精度、鲁棒性和环境适应性，使无人机在近地面无 GPS 环境、隧道 / 室内巡检、高空长航时任务等场景中能稳定飞行和精准作业。
⛳️ 运行结果
📣 部分代码
%% load data
% load data.mat
load outdoorDynamicData.mat
% delete unuseable variables
clearvars -except IMU IMU_label MAG MAG_label EKF1 EKF1_label GPS GPS_label BARO BARO_label ;
%% find IMUStart
% data length
[IMULength,~] = size(IMU);
indexIMU = 1;
[MAGLength,~] = size(MAG);
indexMAG = 1;
[GPSLength,~] = size(GPS);
indexGPS = 1;
[BAROLength,~] = size(BARO);
indexBARO = 1;
%% calculateIniatialtion
% gravityNED
gravityNED = single([0;0;9.807]);
% earthRateNED,the NED earth spin vector in rad/sec
deg2rad = single(pi/180);
earthRateECEF = single([0, 0, 7.2921e-005]);% ECEF坐标系下地球旋转角速度
for i = 1:length(GPS)
earthRateNED(i,1) = single(cos(GPS(i,8)*deg2rad)*earthRateECEF(3));
earthRateNED(i,2) = single(0);
earthRateNED(i,3) = single(-sin(GPS(i,8)*deg2rad)*earthRateECEF(3));
end
earthRateNED =earthRateNED';
%% ReadMeasurements
%% readIMUData
% angRate =IMU(:,3:5);
% accel=IMU(:,6:8);
%% readGpsData
% ConvertGpsData(GPS_DataArrived,RefLatLongDeg,LatLongDeg,GndSpd,CourseDeg,VelD);
deg2rad = single(pi/180);
earthRadius = single(6378145); %地球半径
% PosNE = single(zeros(2,1));% 2*1
% VelNED = single(zeros(3,1));% 3*1
GndSpd = GPS(:,12);
CourseDeg = GPS(:,13);
VelD= GPS(:,14);
% LatDelta = single(zeros(1,672));
% LongDelta = single(zeros(1,672));
for i= 2:length(GPS)
LatDelta(i) = GPS(i,8) - GPS(1,8);
LongDelta(i) = GPS(i,9) - GPS(1,9);
PosNE(1,i) = earthRadius * LatDelta(i)/100;% m
PosNE(2,i) = earthRadius * cos(GPS(1,8)*deg2rad) * LongDelta (i)/100;% m
VelNED(1,i) = GndSpd(i)*cos(CourseDeg(i)*deg2rad);% m/s
VelNED(2,i) = GndSpd(i)*sin(CourseDeg(i)*deg2rad);% m/s
VelNED(3,i) = VelD(i);% m/s
end
VelNED = VelNED';
PosNE = PosNE';
%% readHgtData
Alt_BARO = BARO(:,3)-BARO(1,3);
Alt_GPS = GPS(:,11)-GPS(1,11);
%% readMagData
Offset = MAG(:,6:8);
Mag = MAG(:,3:5);
magBias = Offset * 0.001;
magData = Mag* 0.001 - magBias;%机体系X/Y/Z
%% CalculateInitialStates
%% EulAHRS
accel = IMU(1, 6:8);
init_roll = atan2(-accel(2), -accel(3));
init_pitch = atan2(accel(1), -accel(3));
mag = MAG(indexMAG, 3:5);
hx = mag(1)*cos(init_pitch) + mag(2)*sin(init_pitch)*sin(init_roll) + mag(3)*sin(init_pitch)*cos(init_roll);
hy = mag(2)*cos(init_roll) - mag(3)*sin(init_roll);
init_yaw = atan2(-hy, hx);
if(init_yaw < 0)
init_yaw = init_yaw + 2*pi;
end
DCM = getDCMFromEuler(init_roll, init_pitch, init_yaw);
MagNED = DCM *magData';
magHeading = atan2(MagNED(2,1), MagNED(1,1));
init_yaw = init_yaw-magHeading;
%% states——16
states = zeros(16,1);% 16*1
% q0 q1 q2 q3
q0 = 0.5*sqrt(1+DCM(1,1)+DCM(2,2)+DCM(3,3));
q1 = 0.5*sqrt(1+DCM(1,1)-DCM(2,2)-DCM(3,3))*sign(DCM(3,2)-DCM(2,3));
q2 = 0.5*sqrt(1-DCM(1,1)+DCM(2,2)-DCM(3,3))*sign(DCM(1,3)-DCM(3,1));
q3 = 0.5*sqrt(1-DCM(1,1)-DCM(2,2)+DCM(3,3))*sign(DCM(2,1)-DCM(1,2));
Quat = normalizeQuaternion([q0,q1,q2,q3]);
% % MagNED
% MagNED = DCM *magData' ;% 3*1332
% %MagNED = DCM*magData' ;
% MagNED = MagNED';
% windVelNE;
% windVelNE = [0;0];
% % MagXYZ
% magData = magData;
% PosNE
PosNE = PosNE;
% VelNED
VelNED = VelNED;
% Alt
Alt_GPS = Alt_GPS;
% Delta_Angle_bias_X_Y_Z
DeAnglebias = [0;0;0];
% Delta_Vel_bias_X_Y_Z
DeVelbias = [0;0;0];
%% initStates
states(1:4,:) = Quat;
states(5:7,:) = VelNED(1,:);
states(8:9,:) = PosNE(1,:);
%states(10) = Alt_BARO(1);
states(10) = Alt_GPS(1);
states(11:13,:) = DeAnglebias(1,:);
states(14:16,:) = DeVelbias(1,:);
% states(17:19,:) = MagNED(1,:);
% states(20:22,:) = magData(1,:);
%% initCovariance
P =calculateInitialData();