三坐标的跟踪系统.m

三坐标的跟踪系统.m 此程序仿真了一个三坐标的跟踪系统

%此程序仿真了一个三坐标的跟踪系统，假设目标的高度不变
%在水平方向匀速飞行,x轴方向和y轴方向的速度分别为200m/s,150m/s,初始坐标为(100公里,50公里,1000米)
%此雷达同时具有测距和测速功能,所以滤波采用的是非线性滤波
T=2;%雷达扫描周期为2秒
x0=10e4;%目标在x方向的初始位置为100公里
y0=5e4;%目标在y方向的初始位置为50公里
z0=1000;%目标在z方向的初始高度为1000米
v0x=200;
v0y=150;
v0z=0;
t=0:T:400-T;%观测时间
x=x0+v0x*t;%目标在x轴上的真实运动轨迹
y=y0+v0y*t;%目标在y轴上的真实运动轨迹
z=z0+zeros(1,200);%目标在z轴上的真实运动轨迹
% figure(1)
% plot3(x,y,z);zoom on;grid on;
% title('目标在空间中的真实飞行轨迹')
deta_r=300;%观测距离的标准方差
deta_alpha=0.1*10^(-3);%观测方位角的标准方差
deta_beta=0.1*10^(-3);%观测仰角的标准方差
deta_v=5;%观测的径向速度的标准方差
v=zeros(1,200);
for k=1:200
v(k)=v0x.*x(k)/sqrt(x(k).^2+y(k).^2+z(k).^2)+v0y.*y(k)/sqrt(x(k).^2+y(k).^2+z(k).^2)+v0z.*z(k)/sqrt(x(k).^2+y(k).^2+z(k).^2);%理想情况下的径向速度.
end
figure(2)
plot(v);grid on;
deta_r=300;%观测距离的标准方差
deta_alpha=0.1*10^(-3);%观测方位角的标准方差
deta_beta=0.1*10^(-3);%观测仰角的标准方差,假设这3个都不变
k=1:200;
deta_x2(k)=deta_r^2*[x(k).^2/(x(k).^2+y(k).^2+z(k).^2)]+y(k).^2*deta_alpha^2+z(k).^2.*x(k).^2/(x(k).^2+y(k).^2)*deta_beta^2;
deta_y2(k)=deta_r^2*[y(k).^2/(x(k).^2+y(k).^2+z(k).^2)]+x(k).^2*deta_alpha^2+z(k).^2.*y(k).^2/(x(k).^2+y(k).^2)*deta_beta^2;
deta_z2(k)=deta_r^2*[z(k).^2/(x(k).^2+y(k).^2+z(k).^2)]+(x(k).^2+y(k).^2)*deta_beta^2;%这三个分别为在三个轴上的观测误差
noise=randn(1,200);
noise=[noise-mean(noise)]/sqrt(var(noise));%产生一个(0,1)分布的高斯白噪声
guance_x=x+sqrt(deta_x2).*noise;%模拟在x方向上的观测值
noise=randn(1,200);
noise=[noise-mean(noise)]/sqrt(var(noise));
guance_y=y+sqrt(deta_y2).*noise;%模拟在y方向上的观测值
noise=randn(1,200);
noise=[noise-mean(noise)]/sqrt(var(noise));
guance_z=z+sqrt(deta_z2).*noise;%模拟在z方向上的观测值
noise=randn(1,200);
noise=[noise-mean(noise)]/sqrt(var(noise));
guance_v=v+deta_v*noise;
% % figure(2)
% % plot(guance_v);grid on;
% % xlabel('点数')
% % ylabel('幅值')
% % title('观测径向速度')
guance=zeros(4,1,200);
for k=1:200
guance(:,:,k)=[guance_x(k) guance_y(k) guance_z(k) guance_v(k)]';
end
% figure(2)
% plot(guance_x);zoom on;grid on;
% xlabel('点数')
% ylabel('距离')
% title('目标在x方向上的观测值')
% figure(3)
% plot(guance_y);zoom on;grid on;
% xlabel('点数')
% ylabel('距离')
% title('目标在y轴方向的距离观测值')
% figure(4)
% plot(guance_z);zoom on;grid on;
% xlabel('点数')
% ylabel('距离')
% title('目标在z轴方向的距离观测值')
%初始化
X=zeros(6,1,200);
XX=zeros(6,1,200);
X(:,:,1)=[guance_x(1) guance_y(1) guance_z(1) (guance_x(2)-guance_x(1))/T (guance_y(2)-guance_y(1))/T (guance_z(2)-guance_z(1))/T]';
X(:,:,2)=[guance_x(2) guance_y(2) guance_z(2) (guance_x(2)-guance_x(1))/T (guance_y(2)-guance_y(1))/T (guance_z(2)-guance_z(1))/T]';
D=[1 0 0 T 0 0;0 1 0 0 T 0;0 0 1 0 0 T;0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1];
%对P0进行初始化
p11=deta_r^2*x(2)^2/(x(2)^2+y(2)^2+z(2)^2)+y(2)^2*deta_alpha^2+x(2)^2*z(2)^2/[x(2)^2+y(2)^2]*deta_beta^2;
p12=x(2)*y(2)/(x(2)^2+y(2)^2)*[deta_r^2*(x(2)^2+y(2)^2)/(x(2)^2+y(2)^2+z(2)^2)-(x(2)^2+y(2)^2)*deta_alpha^2+z(2)^2*deta_beta^2];
p13=x(2)*z(2)/(x(2)^2+y(2)^2+z(2)^2)*deta_r^2-x(2)*z(2)*deta_beta^2;
p14=p11/T;
p15=p12/T;
p16=p13/T;
p21=p12;
p22=deta_r^2*y(2)^2/(x(2)^2+y(2)^2+z(2)^2)+x(2)^2*deta_alpha^2+y(2)^2*z(2)^2/(x(2)^2+y(2)^2)*deta_beta^2;
p23=y(2)*z(2)/(x(2)^2+y(2)^2+z(2)^2)*deta_r^2-y(2)*z(2)*deta_beta^2;
p24=p12/T;
p25=p22/T;
p26=p23/T;
p31=p13;
p32=p23;
p33=deta_r^2*z(2)^2/(x(2)^2+y(2)^2+z(2)^2)+(x(2)^2+y(2)^2)*deta_beta^2;
p34=p13/T;
p35=p23/T;
p36=p33/T;
p41=p14;
p42=p24;
p43=p34;
p44=1/T^2*([deta_r^2*x(2)^2/(x(2)^2+y(2)^2+z(2)^2)+y(2)^2*deta_alpha^2+x(2)^2*z(2)^2/(x(2)^2+y(2)^2)]+[deta_r^2*x(1)^2/(x(1)^2+y(1)^2+z(1)^2)+y(1)^2*deta_alpha^2+x(1)^2*z(1)^2/(x(1)^2+y(1)^2)]);
p45=1/T^2*([deta_r^2*x(2)*y(2)/(x(2)^2+y(2)^2+z(2)^2)-x(2)*y(2)*deta_alpha^2+z(2)^2*x(2)*y(2)/(x(2)^2 +y(2)^2)*deta_beta^2] +[deta_r^2*x(1)*y(1)/(x(1)^2+y(1)^2+z(1)^2)-x(1)*y(1)*deta_alpha^2+z(1)^2 *x(1)*y(1)/(x(1)^2+y(1)^2)*deta_beta^2]);
p46=1/T^2*([x(2)*z(2)/(x(2)^2+y(2)^2+z(2)^2)*deta_r^2-x(2)*z(2)*deta_beta^2]+[x(1)*z(1)/(x(1)^2+y(1)^2+z(1)^2)*deta_r^2-x(1)*z(1)*deta_beta^2]);
p51=p15;
p52=p25;
p53=p35;
p54=p45;
p55=1/T^2*([deta_r^2*y(2)^2/(x(2)^2+y(2)^2+z(2)^2)+x(2)^2*deta_alpha^2+y(2)^2*z(2)^2/(x(2)^2+y(2)^2)] +[deta_r^2*y(1)^2/(x(1)^2+y(1)^2+z(1)^2)+x(1)^2*deta_alpha^2+y(1)^2*z(1)^2/(x(1)^2+y(1)^2)]);
p56=1/T^2*([y(2)*z(2)/(x(2)^2+y(2)^2+z(2)^2)*deta_r^2-y(2)*z(2)*deta_beta^2]+[y(1)*z(1)/(x(1)^2+y(1)^2+z(1)^2)*deta_r^2-y(1)*z(1)*deta_beta^2]);
p61=p16;
p62=p26;
p63=p36;
p64=p46;
p65=p56;
p66=1/T^2*([deta_r^2*z(2)^2/(x(2)^2+y(2)^2+z(2)^2)+(x(2)^2+y(2)^2)*deta_beta^2]+[deta_r^2*z(1)^2/(x(1)^2 +y(1)^2+z(1)^2)+(x(1)^2+y(1)^2)*deta_beta^2]);
P0=[p11 p12 p13 p14 p15 p16;p21 p22 p23 p24 p25 p26;p31 p32 p33 p34 p35 p36;p41 p42 p43 p44 p45 p46;p51 p52 p53 p54 p55 p56;p61 p62 p63 p64 p65 p66];
P=zeros(6,6,200);
PP=zeros(6,6,200);
P(:,:,2)=P0;
I=[1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0;0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1];
K=zeros(6,4,200);
R=zeros(4,4,200);
H=zeros(4,6,200);
H1=zeros(1,1,200);
H2=zeros(1,1,200);
H3=zeros(1,1,200);
H4=zeros(1,1,200);
H5=zeros(1,1,200);
H6=zeros(1,1,200);
k=1:200;
deta_xy2(k)=(x(k).*y(k))/(x(k).^2+y(k).^2).*[deta_r^2*(x(k).^2+y(k).^2)/(x(k).^2+y(k).^2+z(k).^2)-(x(k).^2+y(k).^2)*deta_alpha^2+z(k).^2*deta_beta^2];
deta_xz2(k)=(x(k).*z(k))/(x(k).^2+y(k).^2+z(k).^2)*deta_r^2-x(k).*z(k)*deta_beta^2;
deta_yz2(k)=(y(k).*z(k))/(x(k).^2+y(k).^2+z(k).^2)*deta_r^2-y(k).*z(k)*deta_beta^2;
for k=1:200
R(:,:,k)=[deta_x2(k) deta_xy2(k) deta_xz2(k) 0;deta_xy2(k) deta_y2(k) deta_yz2(k) 0;deta_xz2(k) deta_yz2(k) deta_z2(k) 0;0 0 0 deta_v^2];%观测误差的协方差矩阵
end
radius=10000;
m=zeros(1,1,200);
for k=2:199
XX(:,:,k+1)=D*X(:,:,k);
PP(:,:,k+1)=D*P(:,:,k)*D';
H1(k+1)=[XX(4,1,k+1)*(XX(2,1,k+1)^2+XX(3,1,k+1)^2)-XX(5,1,k+1)*XX(1,1,k+1)*XX(2,1,k+1)-XX(6,1,k+1)*XX(1,1,k+1)*XX(3,1,k+1)]/(XX(1,1,k+1)^2+XX(2,1,k+1)^2+XX(3,1,k+1)^2)^(3/2);
H2(k+1)=[XX(5,1,k+1)*(XX(1,1,k+1)^2+XX(3,1,k+1)^2)-XX(4,1,k+1)*XX(1,1,k+1)*XX(2,1,k+1)-XX(6,1,k+1)*XX(2,1,k+1)*XX(3,1,k+1)]/(XX(1,1,k+1)^2+XX(2,1,k+1)^2+XX(3,1,k+1)^2)^(3/2);
H3(k+1)=[XX(6,1,k+1)*(XX(1,1,k+1)^2+XX(2,1,k+1)^2)-XX(4,1,k+1)*XX(1,1,k+1)*XX(3,1,k+1)-XX(5,1,k+1)*XX(2,1,k+1)*XX(3,1,k+1)]/(XX(1,1,k+1)^2+XX(2,1,k+1)^2+XX(3,1,k+1)^2)^(3/2);
H4(k+1)=XX(1,1,k+1)/(XX(1,1,k+1)^2+XX(2,1,k+1)^2+XX(3,1,k+1)^2)^(1/2);
H5(k+1)=XX(2,1,k+1)/(XX(1,1,k+1)^2+XX(2,1,k+1)^2+XX(3,1,k+1)^2)^(1/2);
H6(k+1)=XX(3,1,k+1)/(XX(1,1,k+1)^2+XX(2,1,k+1)^2+XX(3,1,k+1)^2)^(1/2);
H(:,:,k+1)=[1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0;H1(k+1) H2(k+1) H3(k+1) H4(k+1) H5(k+1) H6(k+1)];
K(:,:,k+1)=PP(:,:,k+1)*H(:,:,k+1)'*[H(:,:,k+1)*PP(:,:,k+1)*(H(:,:,k+1))'+R(:,:,k+1)]^(-1);
P(:,:,k+1)=[I-K(:,:,k+1)*H(:,:,k+1)]*PP(:,:,k+1);
vv(k+1)=XX(1,1,k+1)*XX(4,1,k+1)/[(XX(1,1,k+1))^2+(XX(2,1,k+1))^2+(XX(3,1,k+1))^2]^(1/2)+XX(2,1,k+1)*XX(5,1,k+1)/[(XX(1,1,k+1))^2+(XX(2,1,k+1))^2+(XX(3,1,k+1))^2]^(1/2)+XX(3,1,k+1)*XX(6,1,k+1)/[(XX(1,1,k+1))^2+(XX(2,1,k+1))^2+(XX(3,1,k+1))^2]^(1/2);
h(:,:,k+1)=[XX(1,1,k+1) XX(2,1,k+1) XX(3,1,k+1) vv(k+1)]';
m(k+1)=sqrt((guance(1,1,k+1)-XX(1,1,k+1))^2+(guance(2,1,k+1)-XX(2,1,k+1))^2+(guance(3,1,k+1)-XX(3,1,k+1))^2);
if m(k+1)>radius
X(:,:,k+1)=[guance_x(k+1) guance_y(k+1) guance_z(k+1) (guance_x(k+1)-guance_x(k))/T (guance_y(k+1)-guance_y(k))/T (guance_z(k+1)-guance_z(k))/T]';
else
X(:,:,k+1)=XX(:,:,k+1)+K(:,:,k+1)*[guance(:,:,k+1)-h(:,:,k+1)];
end
end
x_filter=zeros(1,200);
y_filter=zeros(1,200);
z_filter=zeros(1,200);
vx_filter=zeros(1,200);
vy_filter=zeros(1,200);
vz_filter=zeros(1,200);
k=1:200;
x_filter2(k)=X(1,1,k);
y_filter2(k)=X(2,1,k);
z_filter2(k)=X(3,1,k);
vx_filter2(k)=X(4,1,k);
vy_filter2(k)=X(5,1,k);
vz_filter2(k)=X(6,1,k);
figure(1)
plot(guance_x);zoom on;grid on;
hold on;
plot(x_filter2,'r:');zoom on;grid on;
xlabel('点数')
ylabel('距离')
title('经过卡尔曼滤波以后的x方向上的距离')
text
figure(2)
plot(guance_y);zoom on;grid on;
hold on;
plot(y_filter2,'r:');zoom on;grid on;
xlabel('点数')
ylabel('距离')
title('经过卡尔曼滤波以后的y方向上的距离')
figure(3)
plot(guance_z);zoom on;grid on;
hold on;
plot(z_filter2,'r:');zoom on;grid on;
xlabel('点数')
ylabel('距离')
title('经过卡尔曼滤波以后的z方向上的距离')
figure(4)
plot(vx_filter1);zoom on;grid on;
hold on;
plot(vx_filter2,'r:');zoom on;grid on;
xlabel('点数')
ylabel('速度')
title('x方向上的速度滤波值')
figure(5)
plot(vy_filter1);zoom on;grid on;
hold on;
plot(vy_filter2,'r:');zoom on;grid on;
xlabel('点数')
ylabel('速度')
title('y方向上的速度滤波值')
figure(6)
plot(vz_filter1);zoom on;grid on;
hold on;
plot(vz_filter2,'r:');zoom on;grid on;
xlabel('点数')
ylabel('速度')
title('z方向上的速度滤波值')
error_x2=zeros(1,200);
error_y2=zeros(1,200);
error_z2=zeros(1,200);
error_vx2=zeros(1,200);
error_vy2=zeros(1,200);
error_vz2=zeros(1,200);
k=1:200;
error_x2(k)=P(1,1,k);
error_y2(k)=P(2,2,k);
error_z2(k)=P(3,3,k);
error_vx2(k)=P(4,4,k);
error_vy2(k)=P(5,5,k);
error_vz2(k)=P(6,6,k);
figure(7)
plot(sqrt(error_x1));zoom on;grid on;
hold on;
plot(sqrt(error_x2),'r:');zoom on;grid on;
xlabel('点数')
ylabel('均方误差')
title('位置x分量的滤波均方误差')
figure(8)
plot(sqrt(error_y1));zoom on;grid on;
hold on;
plot(sqrt(error_y2),'r:');zoom on;grid on;
xlabel('点数')
ylabel('均方误差')
title('位置y分量的滤波均方误差')
figure(9)
plot(sqrt(error_z1));zoom on;grid on;
hold on;
plot(sqrt(error_z2),'r:');zoom on;grid on;
xlabel('点数')
ylabel('均方误差')
title('位置z分量的滤波均方误差')
figure(10)
plot(sqrt(error_vx1));zoom on;grid on;
hold on;
plot(sqrt(error_vx2),'r:');zoom on;grid on;
xlabel('点数')
ylabel('均方误差')
title('速度x分量的滤波均方误差')
figure(11)
plot(sqrt(error_vy1));zoom on;grid on;
hold on;
plot(sqrt(error_vy2),'r:');zoom on;grid on;
xlabel('点数')
ylabel('均方误差')
title('速度y分量的滤波均方误差')
figure(12)
plot(sqrt(error_vz1));zoom on;grid on;
hold on;
plot(sqrt(error_vz2),'r:');zoom on;grid on;
xlabel('点数')
ylabel('均方误差')
title('速度z分量的滤波均方误差')