无人机java定位 无人机路径跟踪

1 简介

在近代控制中,需要对某一特定的运动目标进行跟踪监测,并需要对其下一时间的运动位置,运动方向,速度等信息进行预先估算,才能达到对目标的即时控制.本文简要讨论了用卡尔曼滤波算法对单个目标航迹进行预测,并借助于Matlab仿真工具,对实验的效果进行评估.

2 部分代码

clc; close all;
%initial state
xo=25;
vox=2;
%Observations ****
% X-Direction Calculation:
X=[4000 4260 4550 4860 5410 5600 5990 6400 6790 7000 ]; %position-X
V=[ 280 282 285 286 290 292 294 296 299 302]; % Velocity in 'X' direction
%X = xlsread('DroneFlightTrace1.xlsx','R2:R700');
%V = 0.07 .*[X];
%Process Errors in Process Covaiance Matrix
del_px=20; %initial covariance matrix is choosen intuitively
del_pv=5;
%initial conditions
acc_x=2;
del_t=1;
vx=2;
del_x=25; %uncertainity in the measurement
%Observation Error
del_X=25;
del_VX=6;
Xk=[];
%The Predicted State
A=[1 del_t;0 1];
B=[(0.5*((del_t).^2));del_t];
p=[];
for k=1:1:length(X)-1;
Xk_= [X(k);V(k)];
uk=acc_x;
Xkp1=((A*Xk_));
Xkp2=((B*uk));   
Xkp=(Xkp1+Xkp2);%this is our first estimation
p=[p;Xkp];
%Initialising Process Covariance Matrix
Pk_=[((del_px).^2) 0;0 ((del_pv).^2)];
%Predicted Process Covariance Matrix
Pkp1=((A)*(Pk_));
Pkp2=((Pkp1)*(A'));
pkp=(Pkp2-[0 Pkp2(2);Pkp2(3) 0]); %since the 2nd and 3rd term are not imp.
%Calculating the Kalman gain
R=[((del_X)^2) 0;0 ((del_VX)^2)];
H=[1 0 ; 0 1];
K=((pkp)*H')/((H*pkp*H')+R);
%The New Observation
k=k+1;
Ykm=[X(k);V(k)];
C=[1 0;0 1];
Yk=C*Ykm;
%Calculating the Current State
Xk=[Xk; Xkp + K*(Yk-(H*(Xkp)))];
%Updating the process covariance matrix
Pk1=((eye)-(K*H))*pkp;
pk=(Pk1-[0 Pk1(3);Pk1(2) 0]);
k=k+1;
end
Xkf=[X(1)];
Vkf=[V(1)];
for k=1:2:(length(Xk)-1)
    Xkf=[Xkf;Xk(k)];
end
for k=2:2:(length(Xk))
    Vkf=[Vkf;Xk(k)];
end
prx=[X(1)];
prv=[V(1)];
for i=1:2:(length(p)-1)
    prx=[prx;p(i)];
end
for i=2:2:(length(p))
    prv=[prv;p(i)];
end
%%% Y-D Calculations:
Y1=[1200 1300 1480 1590 1700 1800 1990 2090 2200 2300]; %Position in Y
V1=[20 22 23 25 27 29 32 34 37 40];% Velocity in 'Y' direction
%Y1 = xlsread('DroneFlightTrace1.xlsx','S2:S700');
%V1 = 0.07.*[Y1];
%Process Errors in Process Covaiance Matrix
del_py=20; %initial covariance matrix is choosen intuitively
del_pv1=5;
%initial conditions
acc_x=2;
acc_y=2;
del_t=1;
vy=2;
del_y=25; %uncertainity in the measurement
%Observation Error
del_Y=25;
del_VY=6;
Yk=[];
%The Predicted State
A=[1 del_t;0 1];
fprintf A;
B=[(0.5*((del_t).^2));del_t];
fprintf B;
q=[];
fprintf q;
for k=1:1:length(Y1)-1;
Yk_= [Y1(k);V1(k)];
uk2=acc_x;
Ykp1=((A*Yk_));
Ykp2=((B*uk2));   
Ykp=(Ykp1+Ykp2);%this is our first estimation
q=[q;Ykp];
%Initialising Process Covariance Matrix
Pk2_=[((del_py).^2) 0;0 ((del_pv1).^2)];
%Predicted Process Covariance Matrix
Pkp12=((A)*(Pk2_));
Pkp22=((Pkp12)*(A'));
pkp2=(Pkp22-[0 Pkp22(2);Pkp22(3) 0]); %since the 2nd and 3rd term are not imp.
%Calculating the Kalman gain
R1=[((del_Y)^2) 0;0 ((del_VY)^2)];
H=[1 0 ; 0 1];
K=((pkp2)*H')/((H*pkp2*H')+R1);
%The New Observation
k=k+1;
Ykm1=[Y1(k);V1(k)];
C=[1 0;0 1];
Yk1=C*Ykm1;
%Calculating the Current State
Yk=[Yk; Ykp + K*(Yk1-(H*(Ykp)))];
Pk12=((eye)-(K*H))*pkp2;
%Updating the process covariance matrix
pk2=(Pk12-[0 Pk12(3);Pk12(2) 0]);
k=k+1;
end
Ykf=[Y1(1)];
Vkf1=[V1(1)];
for k=1:2:(length(Yk)-1)
    Ykf=[Ykf;Yk(k)];
end
for k=2:2:(length(Yk))
    Vkf1=[Vkf1;Yk(k)];
end
pry=[Y1(1)];
prv1=[V1(1)];
for i=1:2:(length(q)-1)
    pry=[pry;q(i)];
end
for i=2:2:(length(q))
    prv1=[prv1;q(i)];
end
%%%% Z-D Calculations:
%Z = xlsread('DroneFlightTrace1.xlsx','T2:T700');
%V3 = 0.07.*[Z];
Z=[80 85 90 95 100 115 130 145 150 165]; %Position 
V3 =[7.2 7.4 7.6 7.9 8.1 8.3 8.5 8.7 8.9 9.0]; %Velocity
%Pr ocess Errors in Process Covaiance Matrix
del_pz=10; %initial covariance matrix is choosen intuitively
del_pv3=5;
%initial conditions
acc_z=2;
del_t=1;
v3z=2;
del_z=12; %uncertainity in the measurement
%Observation Error
del_Z=25;
del_VZ=6;
Zk=[];
end
Zkf=[Z(1)];
V3kf=[V3(1)];
for k=1:2:(length(Zk)-1)
    Zkf=[Zkf;Zk(k)];
end
for k=2:2:(length(Zk))
    V3kf=[V3kf;Zk(k)];
end
prZ=[Z(1)];
prv3=[V3(1)];
for i=1:2:(length(r)-1)
    prZ=[prZ;r(i)];
end
for i=2:2:(length(r))  
    prv3=[prv3;r(i)];
end
E=[];
for i=1:1:(length(X))
    E1=abs(((Xkf(i)-X(i))/Xkf(i))*100);
end
for i=1:1:(length(Y1))
    E2=abs(((Ykf(i)-Y1(i))/Ykf(i))*100);
end
for i=1:1:(length(Z))
    E3=abs(((Zkf(i)-Z(i))/Zkf(i))*100);
end
E = [E,E1,E2,E3];
error = mean(E);
%Plotting data
%t=1:1:(length(X));
figure(1)
 plot(X,'r-*');hold on;
 plot(Xkf,'g--*');hold on;
 plot(prx,'b--o');grid on;
 ylabel('Distance in (m)');
 xlabel('Time[sec]');
 title('X Position Estimate');
 legend('Actual','Estimate','Predicted');
 figure(2)
 plot(Y1,'r-*');hold on;
 plot(Ykf,'g--*');hold on;
 plot(pry,'b--o');grid on;
 ylabel('Distance in (m)');
 xlabel('Time[sec]');
 title('Y Position Estimate');
 legend('Actual','Estimate','Predicted ');
figure(3)
 plot(Z,'r-*');hold on;
 plot(Zkf,'g--*');hold on;
 plot(prZ,'b--o');grid on;
 ylabel('Height in (m)');
 xlabel('Time[sec]');
 title('Z Position Estimate');
 legend('Actual height','Estimate','Predicted height');
 figure(4)
 polarplot(X,'r-*');hold on;grid on;
 polarplot(Xkf,'g--*');hold on;

3 仿真结果

4 参考文献

[1]武建, 张雷, 刘艋,等. 基于卡尔曼滤波器的运动目标跟踪的实现[J]. 硅谷, 2013(10):2.