目标定位基于matlab多机EKF+时差和频差无源定位含Matlab源码 2057期

Posted 2022-08-29 海神之光

tags:

篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了目标定位基于matlab多机EKF+时差和频差无源定位含Matlab源码 2057期相关的知识，希望对你有一定的参考价值。

一、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(‘时差定位-距离绝对误差曲线’);