【目标定位】多机EKF+时差和频差无源定位【含Matlab源码 2057期】

💥💥💥💥💥💥💥💥💞💞💞💞💞💞💞💞💞Matlab武动乾坤博客之家💞💞💞💞💞💞💞💞💞💥💥💥💥💥💥💥💥
🚀🚀🚀🚀🚀🚀🚀🚀🚀🚀🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚀🚀🚀🚀🚀🚀🚀🚀🚀🚀

🔊博主简介：985研究生，Matlab领域科研开发者；

🚅座右铭：行百里者，半于九十。

🏆代码获取方式：
优快云 Matlab武动乾坤—代码获取方式

更多Matlab信号处理仿真内容点击👇
①Matlab信号处理（进阶版）

⛳️关注优快云 Matlab武动乾坤，更多资源等你来！！

⛄一、EKF算法简介

在SLAM问题解决方法中，EKF算法是比较常用的经典算法。机器人的自定位过程是一个非线性化的过程，EKF是为了解决Kalman滤波器不能应用于非线性系统而产生的，该滤波算法的主要过程是预测和更新。在预测和更新过程中，EKF算法对原有的系统方程和观测方程进行线性化并得到一个高估计的结果。如果系统中的非线性很弱，EKF也能给出很好的估计结果。

预测时，使用系统模型如下：

式中，z(k+1）表示的是第k+1步中获得的观测量，W （k+1）表示Kalman增益，其中还包括了前向估计在实际中的权重。

实现EKF-SLAM需要以下几方面内容：系统动态方程以及相应的参量，使用的传感器类型和观测方程，根据这些可以得到EKF的相应形式。二维平面中的EKF-SLAM需要知道机器人在X、Y方向的值，还需要知道二维平面下机器人头部朝向与X轴正方向的夹角。

⛄二、部分源代码

clear all;
clc;
T=1; % 采样周期
num=10;% 蒙特卡罗次数
N=1500/T;% 采样次数
t=0:1:N-1;
goal=[100,450,5,0.6,0.2,0.01];
%%%%%备用初始位置1

%%%%%备用初始位置2
x01=[11,11,1,1]‘;
y01=[1,11,11,1]’;
z01=[2 ,1.2,1 ,0.5]‘;
vx0=[0.5 ,0.45,0.45,0.5]’;
vy0=[0.2 ,0.25,0.3 ,0.3]‘;
vz0=[0.01,0.01,0.02,0.02]’;
%角度误差
w=1/3602pi;
xk0=[120;350;5;0.4;0.2;0.01];
[x,y,z,vx,vy,vz,xo1,yo1,zo1,xo2,yo2,zo2,xo3,yo3,zo3,xo4,yo4,zo4,vxo1,vyo1,vzo1,vxo2,vyo2,vzo2,vxo3,vyo3,vzo3,vxo4,vyo4,vzo4]=gj(goal,x01,y01,z01,vx0,vy0,vz0,N,T);
%1.扩展
[time_kj4,kj4xpj,kj4ypj,kj4zpj,kj4rex10,kj4vksv0]=cjkj4(T,N,w,num,x,y,z,vx,vy,vz,xk0,xo1,yo1,zo1,xo2,yo2,zo2,xo3,yo3,zo3,xo4,yo4,zo4);
[x,y,z,vx,vy,vz,xo1,yo1,zo1,xo2,yo2,zo2,xo3,yo3,zo3,xo4,yo4,zo4,vxo1,vyo1,vzo1,vxo2,vyo2,vzo2,vxo3,vyo3,vzo3,vxo4,vyo4,vzo4]=gj(goal,x0,y0,z0,vx0,vy0,vz0,N,T);
%1.扩展
[time_kj4,kj4xpj,kj4ypj,kj4zpj,kj4rex1,kj4vksv]=cjkj4(T,N,w,num,x,y,z,vx,vy,vz,xk0,xo1,yo1,zo1,xo2,yo2,zo2,xo3,yo3,zo3,xo4,yo4,zo4);
figure(1);
plot3(x,y,z,‘b-.’,xo1,yo1,zo1,‘k-.’,xo2,yo2,zo2,‘r-.’,xo3,yo3,zo3,‘m-.’,xo4,yo4,zo4,‘c-.’);
legend(‘目标位置’,‘载机1位置’,‘载机2位置’,‘载机3位置’,‘载机4位置’);
grid;
xlabel(‘x(km)’);
ylabel(‘y(km)’);
zlabel(‘z(km)’);
figure(2);
plot3(x,y,z,‘b-.’,kj4xpj,kj4ypj,kj4zpj,‘r-’);
legend(‘目标位置’,‘定位位置’);
grid;
xlabel(‘x(km)’);
ylabel(‘y(km)’);
zlabel(‘z(km)’)
title(‘EKF定位算法-定位追踪曲线曲线’);

figure(3)
plot(t,kj4rex10,‘r-.’,t,kj4rex1,‘b-’);
legend(‘机间距5km’,‘机间距30km’);
xlabel(‘t’);
ylabel(‘误差(km)’);
grid;
title(‘EKF算法-距离绝对误差曲线’);

figure(4);
plot(t,kj4vksv0,‘r-.’,t,kj4vksv,‘b-’);
legend(‘机间距5km’,‘机间距30km’);
grid; %速度绝对误差曲线
xlabel(‘采样时间t（s）’);
ylabel(‘速度绝对误差(km)’);
title(‘EKF算法-速度绝对误差曲线’);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

x0=[31,31,1,1]‘;
y0=[1,31,31,1]’;
z0=[1 ,1,1 ,1]‘;
vx0=[0.5 ,0.45,0.35,0.47]’;
vy0=[0.1 ,0.1,0.08 ,0.1]‘;
vz0=[0.01,0.01,0.015,0.015]’;
[x,y,z,vx,vy,vz,xo1,yo1,zo1,xo2,yo2,zo2,xo3,yo3,zo3,xo4,yo4,zo4,vxo1,vyo1,vzo1,vxo2,vyo2,vzo2,vxo3,vyo3,vzo3,vxo4,vyo4,vzo4]=gj(goal,x0,y0,z0,vx0,vy0,vz0,N,T);
xii=[x(1)+10,y(1)+10,z(1)];
[Xk,m]=pc(xii,x,y,z,vx,vy,vz,xo1,yo1,zo1,xo2,yo2,zo2,xo3,yo3,zo3,xo4,yo4,zo4,vxo1,vyo1,vzo1,vxo2,vyo2,vzo2,vxo3,vyo3,vzo3,vxo4,vyo4,vzo4,N);
N=300/T;% 采样次数
%%%%%备用初始位置1
x0=[0,30,-30,0]';
y
[x,y,z,vx,vy,vz,xo1,yo1,zo1,xo2,yo2,zo2,xo3,yo3,zo3,xo4,yo4,zo4,vxo1,vyo1,vzo1,vxo2,vyo2,vzo2,vxo3,vyo3,vzo3,vxo4,vyo4,vzo4]=gj(goal,x0,y0,z0,vx0,vy0,vz0,N,T);
xii=[x(1)+1,y(1)+1,z(1)];
[X_g,X_c]=SC(xii,x,y,z,xo1,yo1,zo1,xo2,yo2,zo2,xo3,yo3,zo3,xo4,yo4,zo4,N);
figure(5)
plot3(x,y,z,‘b-.’,Xk(1,:),Xk(2,:),Xk(3,:),‘r-’);
legend(‘目标位置’,‘定位位置’);
grid;
xlabel(‘x(km)’);
ylabel(‘y(km)’);
zlabel(‘z(km)’)
title(‘频差定位算法-定位追踪曲线曲线’);

figure(6)
plot(m);
legend(‘频差定位距离绝对误差曲线’);
xlabel(‘t’);
ylabel(‘误差(m)’);
grid;
title(‘频差定位-距离绝对误差曲线’);

figure(7)
plot3(x,y,z,‘b-.’,X_g(1,:),X_g(2,:),X_g(3,:),‘r-’);
legend(‘目标位置’,‘定位位置’);
grid;
xlabel(‘x(km)’);
ylabel(‘y(km)’);
zlabel(‘z(km)’)
title(‘时差定位算法-定位追踪曲线曲线’);

figure(8)
plot(X_c);
legend(‘时差定位距离绝对误差曲线’);
xlabel(‘t’);
ylabel(‘误差(m)’);
grid;
title(‘时差定位-距离绝对误差曲线’);

⛄三、运行结果

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]林志东.基于扩展卡尔曼滤波算法的SLAM问题分析[J].城市建筑. 2020,17(11)
[2]李超强,李晓波,张靖,曹晨.基于多机协同的机载无源定位分析[J].现代雷达. 2017,39(11)

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 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合