【目标定位】卡尔曼滤波UWB-IMU组合定位导航【含Matlab源码 1601期】

⛄一、简介

针对室内定位中的非视距（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影响下的高精度定位.

⛄二、部分源代码

%%

clc
clear all
close all
global UKF;

addpath(‘ekfukf’);
load(‘ground_truth.mat’)
% Measurement model and it’s derivative
f_func = @ukf_ins_f;
df_dx_func = @ekf_err_ins_f;
h_func = @ekf_uwb_h;
dh_dx_func = @ekf_err_uwb_h;

% anchor position
UKF.BSOneCoordinate = [9.