【滤波跟踪】扩展卡尔曼滤波器IMU和GPS数据计算无人机的姿态【含Matlab源码 2531期】

⛄一、简介

针对室内定位中的非视距（Non-Line-of-Sight,NLOS）现象,提出一个新型算法进行识别,同时有效缓解其影响.主要通过超宽带（Ultra-Wideband,UWB）定位系统与惯性导航系统（Inertial Navigation System,INS）的信息修正非视距误差,获得较高的定位精度.首先,在离线阶段获得不同障碍物下的NLOS误差概率分布曲线;其次,利用惯性测量单元（Inertial Measurement Unit,IMU）的预测位置及NLOS误差概率曲线修正测量距离;最后,利用卡尔曼滤波（Kalman Filtering,KF）融合步行者航迹推算（Pedestrian Dead Reckoning,PDR）的INS位置和经过改进最小二乘法（Least Square,LS）处理后UWB定位系统的位置,并更新NLOS误差获得更准确的位置估计.通过仿真和实验证实了提出的定位算法可以有效缓解NLOS误差,提升定位性能,实现在NLOS影响下的高精度定位.

⛄二、部分源代码

%for testing
clc
clear
close all

pauseLen = 0;

%%Initializations
%TODO: load data here
data = load(‘lib/IMU_GPS_GT_data.mat’);
IMUData = data.imu;
GPSData = data.gpsAGL;
gt = data.gt;

addpath([cd, filesep, ‘lib’])
initialStateMean = eye(5);
initialStateCov = eye(9);
d