% 我給你一個我編的程序。看不懂可以問我，它是
% 是一個目標(biāo)勻速的一個模型，第一種方法是雷達(dá)只有測速功能。用的是標(biāo)準(zhǔn)卡爾曼濾波，第二種是目標(biāo)帶有測速（徑向速度）功能，用的是非線性卡爾曼濾波。希望對你有幫助。
%此程序仿真了一個三坐標(biāo)的跟蹤系統(tǒng)，假設(shè)目標(biāo)的高度不變
%在水平方向勻速飛行,x軸方向和y軸方向的速度分別為200m/s,150m/s,初始坐標(biāo)為(100公里,50公里,1000米)
clear;
clc;
tic;
close all;
T=2;%雷達(dá)掃描周期為2秒
x0=10e4;%目標(biāo)在x方向的初始位置為100公里
y0=5e4;%目標(biāo)在y方向的初始位置為50公里
z0=1000;%目標(biāo)在z方向的初始高度為1000米
v0x=200;
v0y=150;
t=0:T:400-T;%觀測時間
x=x0+v0x*t;%目標(biāo)在x軸上的真實運動軌跡
y=y0+v0y*t;%目標(biāo)在y軸上的真實運動軌跡
z=z0+zeros(1,200);%目標(biāo)在z軸上的真實運動軌跡
figure(13)
plot(x,y);zoom on;grid on;
title('目標(biāo)在空間中的真實飛行軌跡')


deta_r=300;%觀測距離的標(biāo)準(zhǔn)方差
deta_alpha=0.1*10^(-3);%觀測方位角的標(biāo)準(zhǔn)方差
deta_beta=0.1*10^(-3);%觀測仰角的標(biāo)準(zhǔn)方差,假設(shè)這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));%產(chǎn)生一個(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);
guance_z=z+sqrt(deta_z2).*noise;%模擬在z方向上的觀測值
guance=zeros(3,1,200);
for k=1:200
guance(:,:,k)=[guance_x(k) guance_y(k) guance_z(k)]';
end
 figure(2)
 plot(guance_x);zoom on;grid on;
 xlabel('點數(shù)')
 ylabel('目標(biāo)在x軸方向的距離觀測值')
% figure(3)
% plot(guance_y);zoom on;grid on;
% xlabel('點數(shù)')
% ylabel('目標(biāo)在y軸方向的距離觀測值')
% figure(4)
% plot(guance_z);zoom on;grid on;
% xlabel('點數(shù)')
% ylabel('目標(biāo)在z軸方向的距離觀測值')
%初始化;
H=[1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0];%觀測矩陣
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];%一步狀態(tài)轉(zhuǎn)移矩陣
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];%單位矩陣
R=zeros(3,3,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);deta_xy2(k) deta_y2(k) deta_yz2(k);deta_xz2(k) deta_yz2(k) deta_z2(k)];%觀測誤差的協(xié)方差矩陣
end
%對P0進(jìn)行初始化
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);
P(:,:,2)=P0;
PP=zeros(6,6,200);
X=zeros(6,1,200);
XX=zeros(6,1,200);
K=zeros(6,3,200);
X0=[x(2) y(2) z(2) (x(2)-x(1))/T (y(2)-y(1))/T (z(2)-z(1))/T]';
X(:,:,2)=X0;
radius=3000;%跟蹤波門的門限
%卡爾曼濾波
for k=2:199
PP(:,:,k+1)=D*P(:,:,k)*D';
K(:,:,k+1)=PP(:,:,k+1)*H'*[H*PP(:,:,k+1)*H'+R(:,:,k+1)]^(-1);
P(:,:,k+1)=[I-K(:,:,k+1)*H]*PP(:,:,k+1);
XX(:,:,k+1)=D*X(:,:,k);
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]';%如果預(yù)測值與觀測值的門限大于radius,則濾波值用觀測值來代替
else
X(:,:,k+1)=D*X(:,:,k)+K(:,:,k+1)*[guance(:,:,k+1)-H*D*X(:,:,k)];
end
end
x_filter=zeros(1,200);
y_filter=zeros(1,200);
z_filter=zeros(1,200);
k=1:200;
x_filter(k)=X(1,1,k);
y_filter(k)=X(2,1,k);
z_filter(k)=X(3,1,k);
x_filter(1)=guance_x(1);
x_filter(2)=guance_x(2);
y_filter(1)=guance_y(1);
y_filter(2)=guance_y(2);
z_filter(1)=guance_z(1);
z_filter(2)=guance_z(2);
% figure(5)
% plot3(guance_x,guance_y,guance_z);zoom on;grid on;
% title('三維空間中的軌跡觀測值')
% figure(6)
% plot(x_filter);zoom on;grid on;
% xlabel('點數(shù)')
% ylabel('經(jīng)過卡爾曼濾波后的x軸方向的距離')
 figure(7)
 plot(y_filter);zoom on;grid on;
 xlabel('點數(shù)')
 ylabel('經(jīng)過卡爾曼濾波后的y軸方向的距離')
% figure(8)
% plot(z_filter);zoom on;grid on;
% xlabel('點數(shù)')
% ylabel('經(jīng)過卡爾曼濾波后的z軸方向的距離')
% figure(9)
% plot3(x_filter,y_filter,z_filter);zoom on;grid on;
% title('經(jīng)過卡爾曼濾波后的空間軌跡')
% figure(10)
% plot(x_filter,y_filter);zoom on;grid on;
% title('經(jīng)過濾波后目標(biāo)在x-y平面上的運動軌跡')
% figure(11)
% plot(guance_x,guance_y);zoom on;grid on;
% title('目標(biāo)在x-y平面上的觀測軌跡')
vx_filter1=zeros(1,200);
for k=1:200
vx_filter1(k)=X(4,1,k);
vy_filter1(k)=X(5,1,k);
vz_filter1(k)=X(6,1,k);
end
% figure(12)
% plot(vx_filter);zoom on;grid on;
% xlabel('點數(shù)')
% ylabel('速度')
% title('x方向的速度濾波值')
% figure(13)
% plot(vy_filter);zoom on;grid on;
% title('y方向的速度濾波值')
% xlabel('點數(shù)')
% ylabel('速度')
% figure(14)
% plot(vz_filter);zoom on;grid on;
% xlabel('點數(shù)')
% ylabel('速度')
% title('z方向的速度濾波值')
error_x1=zeros(1,200);
error_y1=zeros(1,200);
error_z1=zeros(1,200);
error_vx1=zeros(1,200);
error_vy1=zeros(1,200);
error_vz1=zeros(1,200);
k=1:200;
error_x1(k)=P(1,1,k);
error_y1(k)=P(2,2,k);
error_z1(k)=P(3,3,k);
error_vx1(k)=P(4,4,k);
error_vy1(k)=P(5,5,k);
error_vz1(k)=P(6,6,k);
% figure(15)
% plot(error_x);zoom on;grid on;
% xlabel('點數(shù)')
% ylabel('誤差')
% title('位置x分量的濾波誤差')
% figure(16)
% plot(error_y);zoom on;grid on;
% xlabel('點數(shù)')
% ylabel('誤差')
% title('位置y分量的濾波誤差')
% figure(17)
% plot(error_z);zoom on;grid on;
% xlabel('點數(shù)')
% ylabel('誤差')
% title('位置z分量的濾波誤差')
% figure(18)
% plot(error_vx);zoom on;grid on;
% xlabel('點數(shù)')
% ylabel('誤差')
% title('速度x分量的濾波誤差')
% figure(19)
% plot(error_vy);zoom on;grid on;
% xlabel('點數(shù)')
% ylabel('誤差')
% title('速度y分量的濾波誤差')
% figure(20)
% plot(error_vz);zoom on;grid on;
% xlabel('點數(shù)')
% ylabel('誤差')
% title('速度z分量的濾波誤差')
time=toc