%Kalman Filter
clc
clear
 T=1%雷達(dá)掃描周期
 num=100;%濾波次數(shù)
 al=1/120;%機(jī)動(dòng)頻率常數(shù)
 amax=100;%m/s^2
 amax1=-100;
 %********************產(chǎn)生真實(shí)軌跡************************
 N1=10/T;
 N2=20/T;
 N3=30/T;
 N4=40/T;
 
 sx=zeros(1,N4);%目標(biāo)實(shí)際x位置
 sy=zeros(1,N4);
 svx=zeros(1,N4);%目標(biāo)實(shí)際x方向速度
 svy=zeros(1,N4);
 sax=zeros(1,N4);%目標(biāo)實(shí)際x方向加速度
 say=zeros(1,N4);
 sx(1)=100;%初始x位置
 sy(1)=3000;%初始y位置
 v0=426;%x方向初始速度m/s
 b=pi/4;%目標(biāo)初始航向角
 svx(1)=v0*cos(b);%初始x速度
 svy(1)==v0*sin(b);%初始y速度
 sax=0;%初始x加速度
 say=0;%初始y加速度
 a0=20;%x方向的加速度m/s^2
 
 
 Rr=3000;%%旋轉(zhuǎn)半徑m
 for i=1:N4-1
      if (i>N1&i<N2-1)
       s=v0*T+0.5*a0*T*T;
      v0=v0+a0*T;
      xx=s*cos(b);
      yy=s*sin(b);
      sx(i+1)=sx(i)+xx;
      sy(i+1)=sy(i)+yy;
      svx(i+1)=v0*cos(b);
      svy(i+1)=v0*sin(b); 
      sax(i+1)=a0*cos(b);
      say(i+1)=a0*sin(b);
       elseif (i>N2&i<N3-1)
       a1=v0^2/Rr;
      bb=a1/v0*T;
      xx=Rr*(sin(b+bb)-sin(b));
      yy=Rr*(cos(b)-cos(b+bb));
      b=b+bb;
      sx(i+1)=sx(i)+xx;
      sy(i+1)=sy(i)+yy;
      svx(i)=v0*cos(b);
      svy(i)=v0*sin(b);
      sax(i)=-a1*sin(b);
      say(i)=a1*cos(b);
       elseif (i>N3&i<N4-1)
       a1=v0^2/Rr;
      bb=a1/v0*T;
      xx=Rr*(sin(b-bb)-sin(b));
      yy=Rr*(cos(b)-cos(b-bb));
      b=b-bb;
      sx(i+1)=sx(i)-xx;
      sy(i+1)=sy(i)-yy;
      svx(i)=v0*cos(b);
      svy(i)=v0*sin(b);
      sax(i)=a1*sin(b);
      say(i)=a1*cos(b);
       else
        s=v0*T;
      xx=s*cos(b);
      yy=s*sin(b);
      sx(i+1)=sx(i)+xx;
      sy(i+1)=sy(i)+yy;
      svx(i)=v0*cos(b);
      svy(i)=v0*sin(b);
      sax(i)=0;
      say(i)=0; 
       
     end
       
     
 end
 
 
 

%*****************************************
Qc=[1 T T*T/2 0 0 0;0 1 T 0 0 0;0 0 1 0 0 0;0 0 0 1 T T*T/2;0 0 0 0 1 T;0 0 0 0 0 1];%一步轉(zhuǎn)移輸矩陣
Qb=[1 T 1/al^2*(-1+al*T+exp(-al*T)) 0 0 0;0 1 1/al*(1-exp(-al*T)) 0 0 0;0 0 exp(-al*T) 0 0 0;0 0 0 1 T 1/al^2*(-1+al*T+exp(-al*T));0 0 0 0 1 1/al*(1-exp(-al*T));0 0 0 0 0 exp(-al*T)];
H=[1 0 0 0 0 0;0 0 0 1 0 0];%觀測(cè)矩陣
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];%單位矩陣
  deta_w=1;
   x0=30;
   y0=30;%固定測(cè)量誤差
  
 bl=0.01;%比例系數(shù)
 e2=normrnd(0,sqrt(deta_w),[1 N4]);%第一個(gè)參數(shù)是均值,第二個(gè)是標(biāo)準(zhǔn)差，后面用于指定生成矩陣的大小
 vk=zeros(2,1,N4);%觀測(cè)噪聲V(k)
 Z=zeros(2,1,N4);%觀測(cè)值
 xtzt=zeros(6,1,N4);%系統(tǒng)狀態(tài)
 R=zeros(2,2,N4);%測(cè)噪聲方差
 for i=1:N4
    xtzt(1,1,i)=sx(i);
    xtzt(4,1,i)=sy(i);
 end
 for i=1:N4
 vk(1,1,i)=(bl*sx(i)+x0)*e2(i);%計(jì)算觀測(cè)噪聲V(k)    
 vk(2,1,i)=(bl*sy(i)+y0)*e2(i);
 Z(:,:,i)=H*xtzt(:,:,i)+vk(:,:,i);%觀測(cè)方程
 R(:,:,i)=[(bl*sx(i)+x0)^2*mean(e2(i)*e2(i)) 0 ;0 (bl*sy(i)+y0)^2*mean(e2(i)*e2(i))];%計(jì)算觀測(cè)噪聲方差
 end
 
  for k=1:N4
      guance_x(k)=Z(1,1,k);
      guance_y(k)=Z(2,1,k);
  end
%*****************************************
%計(jì)算Q中的常數(shù)矩陣
a2x=zeros(1,N4);%ax加速度方差
a2y=zeros(1,N4);%ay加速度方差
Q=zeros(6,6,N4);%常數(shù)矩陣
Q11=zeros(3,3,N4);%常數(shù)矩陣
Q22=zeros(3,3,N4);%常數(shù)矩陣
Q12=zeros(3,3,N4);%常數(shù)矩陣

q11=1/(2*al^5)*(1-exp(-2*al*T)+2*al*T+2*al^3*T^3/3-2*al*al*T^2-4*al*T*exp(-al*T));
q12=1/(2*al^4)*(exp(-2*al*T)+1-2*exp(-al*T)+2*al*T*exp(-al*T)-2*al*T+al*al*T^2);
q21=q12;
q13=1/(2*al^3)*(1-exp(-2*al*T)-2*al*T*exp(-al*T));
q31=q13;
q22=1/(2*al^3)*(4*exp(-al*T)-3-exp(-2*al*T)+2*al*T);
%q23=1/(2*al^2)*(exp(-2*al*T)+1-2*al*T);
q23=1/(2*al^2)*(exp(-2*al*T)+1-2*exp(-al*T));
q32=q23;
q33=1/(2*al)*(1-exp(-2*al*T));
q2=[q11 q12 q13;q21 q22 q23;q31 q32 q33];
 
%********************kalman濾波初始化************************
 %3點(diǎn)起始法定初值
 XX1=zeros(6,1,N4);%存放一步預(yù)測(cè)估計(jì)值
PP1=zeros(6,6,N4);%存放預(yù)測(cè)濾波協(xié)方差矩陣
K=zeros(6,2,N4);%存放增益矩陣
Qz=zeros(6,6,N4);%
a2x(3)=10;
a2y(3)=10;

 %X=zeros(6,1,N4);
 XX=zeros(6,1,N4);%存放估計(jì)值
 PP=zeros(6,6,N4);%存放濾波協(xié)方差矩陣
 %XX(:,:,3)=[Z(1,1,3) 3*(Z(1,1,3)-Z(1,1,2)-(Z(1,1,3)-Z(1,1,1)))/T (Z(1,1,3)-2*Z(1,1,2)+Z(1,1,1))/T^2 Z(2,1,3) 3*(Z(2,1,3)-Z(2,1,2)-(Z(2,1,3)-Z(2,1,1)))/T (Z(2,1,3)-2*Z(2,1,2)+Z(2,1,1))/T^2]';%初始狀態(tài)值估計(jì)
 XX(:,:,3)=[403 295 0 3602 370 0];

 %對(duì)PP(:,:,3)進(jìn)行初始化
deta_r=100;%觀測(cè)距離的標(biāo)準(zhǔn)方差
deta_alpha=0.1*10^(-3);%觀測(cè)方位角的標(biāo)準(zhǔn)方差
p11=deta_r^2*sx(3)^2/(sx(3)^2+sy(3)^2)+sy(3)^2*deta_alpha^2;
p12=p11/T;
p13=p11/T^2;
p14=deta_r^2*sx(3)*sy(3)/(sx(3)^2+sy(3)^2)-sx(3)*sy(3)*deta_alpha^2;
p15=p14/T;
p16=p14/T^2;
p21=p12;
p22=(deta_r^2*sx(3)^2/(sx(3)^2+sy(3)^2)+sy(3)^2*deta_alpha^2+deta_r^2*sx(2)^2/(sx(2)^2+sy(2)^2)+sy(2)^2*deta_alpha^2)/T^2;
p23=(deta_r^2*sx(3)^2/(sx(3)^2+sy(3)^2)+sy(3)^2*deta_alpha^2+2*(deta_r^2*sx(2)^2/(sx(2)^2+sy(2)^2)+sy(2)^2*deta_alpha^2))/T^3;
p24=p15;
p25=(deta_r^2*sx(3)*sy(3)/(sx(3)^2+sy(3)^2)-sx(3)*sy(3)*deta_alpha^2+deta_r^2*sx(2)*sy(2)/(sx(2)^2+sy(2)^2)-sx(2)*sy(2)*deta_alpha^2)/T^2;
p26=(deta_r^2*sx(3)*sy(3)/(sx(3)^2+sy(3)^2)-sx(3)*sy(3)*deta_alpha^2+2*(deta_r^2*sx(2)*sy(2)/(sx(2)^2+sy(2)^2)-sx(2)*sy(2)*deta_alpha^2))/T^3;
p31=p11/T^3;
p32=p23;
p33=(deta_r^2*sx(3)^2/(sx(3)^2+sy(3)^2)+sy(3)^2*deta_alpha^2+4*(deta_r^2*sx(2)^2/(sx(2)^2+sy(2)^2)+sy(2)^2*deta_alpha^2)+deta_r^2*sx(1)^2/(sx(1)^2+sy(1)^2)+sy(1)^2*deta_alpha^2)/T^4;
p34=p13/T;
p35=p23/T;
p36=p33/T;
p41=p14;
p42=p24;
p43=p34;
p44=deta_r^2*sy(3)^2/(sx(3)^2+sy(3)^2)+sx(3)^2*deta_alpha^2;
p45=p44/T;
p46=p45/T;
p51=p15;
p52=p25;
p53=p35;
p54=p45;
p55=(deta_r^2*sy(3)^2/(sx(3)^2+sy(3)^2)+sx(3)^2*deta_alpha^2+deta_r^2*sy(2)^2/(sx(2)^2+sy(2)^2)+sx(2)^2*deta_alpha^2)/T^2;
p56=(deta_r^2*sy(3)^2/(sx(3)^2+sy(3)^2)+sx(3)^2*deta_alpha^2+2*(deta_r^2*sy(2)^2/(sx(2)^2+sy(2)^2)+sx(2)^2*deta_alpha^2))/T^3;
p61=p16;
p62=p26;
p63=p36;
p64=p46;
p65=p56;
p66=(deta_r^2*sy(3)^2/(sx(3)^2+sy(3)^2)+sx(3)^2*deta_alpha^2+4*(deta_r^2*sy(2)^2/(sx(2)^2+sy(2)^2)+sx(2)^2*deta_alpha^2)+deta_r^2*sy(1)^2/(sx(1)^2+sy(1)^2)+sx(1)^2*deta_alpha^2);
%PP(:,:,3)=[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];
PP(:,:,3)=[400 0 0 0 0 0;0 250 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0;0 0 0 0 150 0;0 0 0 0 0 400];

%***********************************KALMAN 濾波

for i=3:N4-1
    Q11(:,:,i)=2*al*a2x(i)*q2;%計(jì)算Q矩陣
    Q22(:,:,i)=2*al*a2y(i)*q2;%計(jì)算Q矩陣
    Q(:,:,i)=[Q11(:,:,i) Q12(:,:,i);Q12(:,:,i) Q22(:,:,i)];%計(jì)算Q矩陣
    XX1(:,:,i)=Qc*XX(:,:,i);%計(jì)算一步預(yù)測(cè)估計(jì)值
    PP1(:,:,i)=Qb*PP(:,:,i)*Qb'+Q(:,:,i);%計(jì)算預(yù)測(cè)濾波協(xié)方差矩陣
    K(:,:,i)=PP1(:,:,i)*H'*inv(H*PP1(:,:,i)*H'+R(:,:,i));%計(jì)算增益矩陣
    XX(:,:,i+1)=XX1(:,:,i)+K(:,:,i)*(Z(:,:,i+1)-H*XX1(:,:,i));%計(jì)算估計(jì)值
    PP(:,:,i+1)=(I-K(:,:,i+1)*H)*PP1(:,:,i);%計(jì)算協(xié)方差更新方程
  
     if(XX(3,:,i)>=0)
   a2x(i+1)=(4-pi)/pi*(amax-XX(3,:,i))*(amax-XX(3,:,i));
 elseif(XX(3,:,i)<0)
    a2x(i+1)=(4-pi)/pi*(amax1+XX(3,:,i))*(amax1+XX(3,:,i));
     end
    if(XX(6,:,i)>=0)
    a2y(i+1)=(4-pi)/pi*(amax-XX(6,:,i))*(amax-XX(6,:,i));
 elseif(XX(6,:,i)<0)
   a2y(i+1)=(4-pi)/pi*(amax1+XX(6,:,i))*(amax1+XX(6,:,i));
    end
    
end

x_filter=zeros(1,N4);
y_filter=zeros(1,N4);
ax_filter=zeros(1,N4);
vx_filter=zeros(1,N4);
vy_filter=zeros(1,N4);
ay_filter=zeros(1,N4);
for k=1:N4;
x_filter(k)=XX(1,1,k);
vx_filter(k)=XX(2,1,k);
ax_filter(k)=XX(3,1,k);
y_filter(k)=XX(4,1,k);
vy_filter(k)=XX(5,1,k);
ay_filter(k)=XX(6,1,k);
end
 figure(1);
 plot(sx,sy,'r-',guance_x,guance_y,'b-x');
 hold on
 plot(x_filter,y_filter,'g-h');
 legend('真實(shí)飛行軌跡','觀測(cè)飛行軌跡','估計(jì)飛行軌跡');
 title('目標(biāo)在空間中的真實(shí)飛行軌跡與估計(jì)軌跡')
 figure(2);
 plot(svx,'r-');
 hold on
  plot(vx_filter,'b-x');
 xlabel('T(s)'),ylabel('Y(m/s)');
 title('x方向上的速度值與估計(jì)值')
 figure(4);
 plot(sax,'b-x');
 hold on
  plot(ax_filter,'r-x');

 xlabel('T(s)'),ylabel('Y(m/s2)');
  title('x方向上的加速度值與估計(jì)值')