✅博主简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，Matlab项目合作可私信。
🍎个人主页：海神之光
🏆代码获取方式：
海神之光Matlab王者学习之路—代码获取方式
⛳️座右铭：行百里者，半于九十。
更多Matlab仿真内容点击👇
Matlab图像处理（进阶版）
路径规划（Matlab）
神经网络预测与分类（Matlab）
优化求解（Matlab）
语音处理（Matlab）
信号处理（Matlab）
车间调度（Matlab）
⛄一、简介
针对室内定位中的非视距（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.21;1.08;-0.17];%4.08
UKF.BSTwoCoordinate = [0;0;-1.885];
UKF.BSThreeCoordinate = [0;6.281;-1.37];
UKF.BSFourCoordinate = [1.705;12.88;-2.27];
UKF.BSFiveCoordinate = [9.31;11.59;-0.52];
UKF.BaseS_Position = [UKF.BSOneCoordinate,UKF.BSTwoCoordinate,…
UKF.BSThreeCoordinate,UKF.BSFourCoordinate,…
UKF.BSFiveCoordinate]*30;
UKF.bSPcs = 5;
% download the sensor data
matfile = dir(‘*_HandledFileToMatData.mat’);
if isempty(matfile)
disp(’ None Found *_HandledFileToMatData.mat’)
end
for ki=1:size(matfile)
load(matfile(ki).name)
end
% initialization
ProcessNoiseVariance = [3.9e-04 4.5e-4 7.9e-4; %%%Accelerate_Variance
1.9239e-7, 3.5379e-7, 2.4626e-7;%%%Accelerate_Bias_Variance
8.7e-04,1.2e-03,1.1e-03; %%%Gyroscope_Variance
1.3111e-9,2.5134e-9, 2.4871e-9 %%%Gyroscope_Bias_Variance
];
Q = [
diag(ProcessNoiseVariance(1,:)),zeros(3,12);
zeros(3,3), diag(ProcessNoiseVariance(1,:)),zeros(3,9);
zeros(3,6), diag(ProcessNoiseVariance(3,:)),zeros(3,6);
zeros(3,9), diag(ProcessNoiseVariance(2,:)),zeros(3,3);
zeros(3,12), diag(ProcessNoiseVariance(4,:))];
MeasureNoiseVariance =[2.98e-03,2.9e-03,…
1.8e-03,1.2e-03,…
2.4e-03];%%%%UWB ranging noise
R = diag(MeasureNoiseVariance);
Position_init =[20;100;-1.9]; deta_Position_init = [0;0;0];
Speed_init = [0;0;0]; deta_Speed_init = [0;0;0];
Accelerate_Bias_init = [0;0;0]; deta_Accelerate_Bias_init = [0;0;0];
Gyroscope_Bias_init = [0;0;0]; deta_Gyroscope_Bias_init = [0;0;0];
Quaternion_init = [1,0,0,0]'; deta_Quaternion_init = [0;0;0;0];
% state init x0 and P0
X0 = [Position_init;Speed_init;Accelerate_Bias_init;Gyroscope_Bias_init;Quaternion_init];
StaticBiasAccelVariance =[6.7203e-5, 8.7258e-5, 4.2737e-5];
StaticBiasGyroVariance = [2.2178e-5, 5.9452e-5, 1.3473e-5];
init_c = 0.1;
P0 = [init_ceye(3,3),zeros(3,12);
zeros(3,3) , 1e-2init_ceye(3,3),zeros(3,9);
zeros(3,6), 1e-2init_c* eye(3,3),zeros(3,6);
zeros(3,9), diag(StaticBiasGyroVariance),zeros(3,3);
zeros(3,12), diag( StaticBiasAccelVariance);
]; %%%
% Initial guesses for the state mean and covariance.
X = [2;2;-3;zeros(3,1);10/180pi;-10/180pi;20/180*pi;…
sqrt(StaticBiasAccelVariance’).*randn(3,1);
sqrt(StaticBiasGyroVariance’).*randn(3,1)
];
dX = [zeros(9,1);
sqrt(StaticBiasAccelVariance’).*randn(3,1);
sqrt(StaticBiasGyroVariance’).randn(3,1)];
P = P0;
imu_iter = 1;
uwb_iter = 1;
dt = 0.02;
Pcs = length(SampleTimePoint);
ISPUTCHAR = 0;
% Reserve space for estimates.
MM = zeros(size(X,1),Pcs);
PP = zeros(size(X,1),size(X,1),Pcs);
Invation = zeros(5,Pcs);
UWBXYZ = [];noise=[];
% Estimate with EKF
for k=1:Pcs
uk = [a_vector(imu_iter,:)‘;g_vector(imu_iter,:)’/180pi];
% Track with (nonaugmented) UKF
[X,P] = ukf_predict1(X,P,f_func,Q,[dt;uk]);
if ISPUTCHAR == 1
cprintf('text', 'T: %0.3fs, L [%0.2f %0.2f %0.2f]m, V [%0.3f %0.3f %0.3f]m/s ,Pl [%0.5f %0.5f %0.5f],Pv [%0.5f %0.5f %0.5f]m/s^2\n',...
SampleTimePoint(imu_iter) ,X(1),X(2),X(3),X(4),X(5),X(6),P(1,1),P(2,2),P(3,3),P(4,4),P(5,5),P(6,6));
end
%%% Update
if uwb_iter <= length(UWBBroadTime_vector) && UWBBroadTime_vector(uwb_iter)== SampleTimePoint(imu_iter)
Z_meas = Uwbranging_vector(uwb_iter,:)';
%%%add the noise
outlier = zero_one_distribution(0.1)';
% outlier = zeros(5,1); %% you could ignore annotation without outlier
Z_meas = Z_meas + outlier;
noise = [noise,outlier];
[X,P] = ukf_update1(X,P,Z_meas,h_func,R);
%% is equal with ukf_update1(...)
% [~,H] = dh_dx_func(X);
% %%5个基站同时测距
% K = PH’inv(HPH’+R);
% dX = K*(Z_meas - h_func(X));Invation(:,k) = Z_meas - h_func(X);
% P = P - KHP;
% %%%%%1个基站测距值
% H_ = H(bS,:);
% K = PH_'inv(H_PH_'+R(bS,bS));
% INVT = Z_meas - h_func(X);
% dX = KINVT(bS);
% P = P - KH_*P;
%feedback
% X = X + dX;
%%
if ISPUTCHAR == 1
cprintf('err', 'T: %0.3fs, L [%0.2f %0.2f %0.2f]m, V [%0.3f %0.3f %0.3f]m/s ,Pl [%0.5f %0.5f %0.5f],Pv [%0.5f %0.5f %0.5f]m/s^2\n\n\n',...
UWBBroadTime_vector(uwb_iter) ,X(1),X(2),X(3),X(4),X(5),X(6),P(1,1),P(2,2),P(3,3),P(4,4),P(5,5),P(6,6));
end
uwb_iter = uwb_iter + 1;
end
MM(:,k) = X;
PP(:,:,k) = P;
imu_iter = imu_iter + 1;
end
MM(7:9,:)= MM(7:9,:)/pi180;
noise = noise’;
for uwb_iter=1:4:length(UWBBroadTime_vector)-10
Z_meas = diag(Uwbranging_vector(uwb_iter:uwb_iter+4,:) + noise(uwb_iter:uwb_iter+4,:)) ;
uwbxyz = triangulate(Z_meas);
UWBXYZ = [UWBXYZ,[UWBBroadTime_vector(uwb_iter+2);uwbxyz;TraceData(4(uwb_iter+2)+1,2:4)']];
end
%-------------- figure 1: display trajectory ----------------------%
base = 1;
figure(1)
subplot(311)
plot(TraceData(:,1),TraceData(:,base+1),‘g.’)
hold on
plot(SampleTimePoint(1:Pcs),MM(base,:),‘m’)
plot(UWBXYZ(1,:),UWBXYZ(2,:),‘k’)
title(‘Position x Axis’);xlabel(‘T:s’);ylabel(‘X axis:m’);grid on;
legend(‘Real Trajectory’,‘UWB-IMU Trajectory’,‘UWB Trajectory’)
subplot(312)
plot(TraceData(:,1),TraceData(:,base+2),‘g.’)
hold on
plot(SampleTimePoint(1:Pcs),MM(base+1,:),‘m’)
plot(UWBXYZ(1,:),UWBXYZ(3,:),‘k’)
title(‘Position y Axis’);xlabel(‘T:s’);ylabel(‘Y axis:m’);grid on;
legend(‘Real Trajectory’,‘UWB-IMU Trajectory’,‘UWB Trajectory’)
subplot(313)
plot(TraceData(:,1),TraceData(:,base+3),‘g.’)
hold on
plot(SampleTimePoint(1:Pcs),MM(base+2,:),‘m’)
plot(UWBXYZ(1,:),UWBXYZ(4,:),‘k’)
title(‘Position z Axis’);xlabel(‘T:s’);ylabel(‘Z axis:m’);grid on;
legend(‘Real Trajectory’,‘UWB-IMU Trajectory’,‘UWB Trajectory’)
%-------------- figure 2: display trajectory error -----------------%
base = 1;
figure(2)
subplot(331)
plot(SampleTimePoint(1:Pcs),MM(base,:)'- TraceData(:,base+1),‘m’);
hold on
plot(UWBXYZ(1,:),UWBXYZ(2,:) - UWBXYZ(5,:),‘k’)
title(‘Position x Axis’);xlabel(‘T:s’);ylabel(‘X axis:m’);grid on;
legend(‘UWB-IMU Trajectory Error’,‘UWB Trajectory Error’)
subplot(332)
xvalues1 = -3:0.2:3;
error = MM(base,:)'- TraceData(:,base+1);
hist(error(find(error < 3 & error > -3)),100);
title(‘Position x Axis Error Hist’);grid on;
legend(‘UWB-IMU Trajectory Error’)
h=subplot(333);
error =UWBXYZ(2,:) - UWBXYZ(5,:);
hist(error(find(error < 3 & error > -3)),100);
hp = findobj(h,‘Type’,‘patch’);
set(hp,‘FaceColor’,[0 .5 .5],‘EdgeColor’,‘w’)
title(‘Position x Axis Error Hist’);grid on;
legend(‘UWB Trajectory Error’)
subplot(334)
hold on
plot(SampleTimePoint(1:Pcs),MM(base+1,:)’ - TraceData(:,base+2),‘m’)
plot(UWBXYZ(1,:),UWBXYZ(3,:) - UWBXYZ(6,:),‘k’)
title(‘Position y Axis’);xlabel(‘T:s’);ylabel(‘Y axis:m’);grid on;
legend(‘UWB-IMU Trajectory Error’,‘UWB Trajectory Error’)
subplot(335)
xvalues1 = -3:0.2:3;
error = MM(base+1,:)'- TraceData(:,base+2);
hist(error(find(error < 3 & error > -3)),100);
title(‘Position x Axis Error Hist’);grid on;
legend(‘UWB-IMU Trajectory Error’)
h=subplot(336);
error =UWBXYZ(3,:) - UWBXYZ(6,:);
hist(error(find(error < 3 & error > -3)),100);
hp = findobj(h,‘Type’,‘patch’);
set(hp,‘FaceColor’,[0 .5 .5],‘EdgeColor’,‘w’)
title(‘Position x Axis Error Hist’);grid on;
legend(‘UWB Trajectory Error’)
subplot(337)
hold on
plot(SampleTimePoint(1:Pcs),MM(base+2,:)’ - TraceData(:,base+3),‘m’)
plot(UWBXYZ(1,:),UWBXYZ(4,:) - UWBXYZ(7,:),‘k’)
title(‘Position z Axis’);xlabel(‘T:s’);ylabel(‘Z axis:m’);grid on;
legend(‘UWB-IMU Trajectory Error’,‘UWB Trajectory Error’)
subplot(338)
xvalues1 = -10:0.2:10;
error = MM(base+2,:)'- TraceData(:,base+3);
hist(error(find(error < 10 & error > -10)),100);
titl
%-------------- figure 3: display Speed state -----------------%
base = 4;
figure(3)
subplot(311)
plot(TraceData(:,1),TraceData(:,base+1),‘r*’);grid on
hold on
plot(SampleTimePoint(1:Pcs),MM(base,:),‘k’)
title(‘Speed x Axis’);xlabel(‘T:s’);ylabel(‘x axis:m’);grid on;
subplot(312)
plot(TraceData(:,1),TraceData(:,base+2),‘g*’);grid on
hold on
plot(SampleTimePoint(1:Pcs),MM(base+1,:),‘k’)
title(‘Speed y Axis’);xlabel(‘T:s’);ylabel(‘y axis:m’);grid on;
subplot(313)
plot(TraceData(:,1),TraceData(:,base+3),‘c*’);grid on
hold on
plot(SampleTimePoint(1:Pcs),MM(base+2,:),‘k’)
t
plot(SampleTimePoint(1:Pcs),MM(base+1,:),‘k’)
title(‘Euler’);grid on;
legend(‘Real Atti’,‘UWB-IMU Atti’)
subplot(313)
plot(TraceData(:,1),TraceData(:,base+3),‘c*’)
hold on
plot(SampleTimePoint(1:Pcs),MM(base+2,:),‘k’)
title(‘Euler’);grid on;
legend(‘Real Atti’,‘UWB-IMU Atti’)
%-------------- figure 5: display estimated accel bias ----------%
base = 10;
figure(5)
subplot(311)
plot(TraceData(:,1),TraceData(:,base+1),‘r*’)
hold on
plot(SampleTimePoint(1:Pcs),MM(base,:),‘k’)
title(‘Accel Bias’);grid on;
legend(‘Real Error’,‘UWB-IMU Error’)
subplot(312)
plot(TraceData(:,1),TraceData(:,base+2),‘g*’)
hold on
plot(SampleTimePoint(1:Pcs),MM(base+1,:),‘k’)
title(‘Accel Bias’);grid on;
legend(‘Real Error’,‘UWB-IMU Error’)
subplot(313)
plot(TraceData(:,1),TraceData(:,base+3),‘c*’)
hold on
plot(SampleTimePoint(1:Pcs),MM(base+2,:),‘k’)
title(‘Accel Bias’);grid on;
legend(‘Real Error’,‘UWB-IMU Error’)
%-------------- figure 6: display estimated gyro bias ----------%
base = 13;
figure(6)
subplot(311)
plot(TraceData(:,1),TraceData(:,base+1),‘r*’)
⛄三、运行结果
⛄四、matlab版本及参考文献
1 matlab版本
2014a
2 参考文献
[1]周军,魏国亮,田昕,王甘楠,融合UWB和IMU数据的新型室内定位算法[J].小型微型计算机系统. 2021,42(08)
3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除
🍅 仿真咨询
1 各类智能优化算法改进及应用
生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化
2 机器学习和深度学习方面
卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断
3 图像处理方面
图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知
4 路径规划方面
旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化
5 无人机应用方面
无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配
6 无线传感器定位及布局方面
传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化
7 信号处理方面
信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化
8 电力系统方面
微电网优化、无功优化、配电网重构、储能配置
9 元胞自动机方面
交通流 人群疏散 病毒扩散 晶体生长
10 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合
3450
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
