kalman.m

程序实现单目标在空间中的真实飞行轨迹与使用小波后对目标的估计轨迹

clc
clear
 T=1%雷达扫描周期
 num=100;%滤波次数
 al=1/120;%机动频率常数
 amax=100;%m/s^2
 amax1=-100;
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%产生真实轨迹%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 N1=10/T;
 N2=20/T;
 N3=33/T;
 N4=47/T;
 
 sx=zeros(1,N4);%目标实际x位置
 sy=zeros(1,N4);
 svx=zeros(1,N4);%目标实际x方向速度
 svy=zeros(1,N4);
 sax=zeros(1,N4);%目标实际x方向加速度
 say=zeros(1,N4);
 sx(1)=100;%初始x位置
 sy(1)=3000;%初始y位置
 v0=426;%x方向初始速度m/s
 b=pi/4;%目标初始航向角
 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;%%旋转半径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
 
 
 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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)];%机动模型为"当前"统计模型
%%%%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];%一步转移输矩阵  滤波模型为a-b-r滤波
H=[1 0 0 0 0 0;0 0 0 1 0 0];%观测矩阵
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;%固定测量误差
  
 bl=0.01;%比例系数
 %e2=normrnd(0,sqrt(deta_w),[1 N4]);%第一个参数是均值,第二个是标准差，后面用于指定生成矩阵的大小
 e2=randn(1,N4);
 vk=zeros(2,1,N4);%观测噪声V(k)
 Z=zeros(2,1,N4);%观测值
 xtzt=zeros(6,1,N4);%系统状态
 R=zeros(2,2,N4);%测噪声方差
 for i=1:N4
    xtzt(1,1,i)=sx(i);%真实轨迹x
    xtzt(4,1,i)=sy(i);%真实轨迹y
 end
 for i=1:N4
 vk(1,1,i)=10*e2(i);%计算观测噪声V(k)    (bl*sx(i)+x0)*
 vk(2,1,i)=10*e2(i);%(bl*sy(i)+y0)*
 Z(:,:,i)=H*xtzt(:,:,i)+vk(:,:,i);%观测方程
 R(:,:,i)=[mean(e2(i)*e2(i)) 0 ;0 mean(e2(i)*e2(i))];%计算观测噪声方差
 end
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%小波重构%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  for i=1:N4
    Zx(i)=Z(1,1,i);
    Zy(i)=Z(2,1,i);
  end
  
 [ZxC,ZxL] = wavedec(Zx,2,'db2');%db1,db2,db3
 [ZyC,ZyL] = wavedec(Zy,2,'db2');

 subplot(3,1,1);
 plot(Zx,Zy);
 title('vk(1,1,i)=10*e2(i)观测轨迹')
  
  %xA3 = waverec(ZxC,ZxL,'db1');
  %yA3 = waverec(ZyC,ZyL,'db1');
  
  xA3 = wrcoef('a',ZxC,ZxL,'db2',2);
  yA3 = wrcoef('a',ZyC,ZyL,'db2',2);
  
  subplot(3,1,2);
  plot(xA3, yA3);
  title('db2小波重构观测轨迹');
  
  subplot(3,1,3);
  plot(sx, sy);
  title('真实轨迹')
  
   w=zeros(2,1,N4);%观测值
for i=1:N4
   w(1,1,i)= xA3(i);
   w(2,1,i)= yA3(i);
end
%subplot(4,1,4);
%plot(w(1,:),w(2,:));
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  for k=1:N4
      guance_x(k)=Z(1,1,k); %观测x坐标
      guance_y(k)=Z(2,1,k); %观测y坐标
  end
  
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%计算Q中的常数矩阵
a2x=zeros(1,N4);%ax加速度方差
a2y=zeros(1,N4);%ay加速度方差
Q=zeros(6,6,N4);%常数矩阵
Q11=zeros(3,3,N4);%常数矩阵
Q22=zeros(3,3,N4);%常数矩阵
Q12=zeros(3,3,N4);%常数矩阵

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滤波初始化%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
XX1=zeros(6,1,N4);%存放一步预测估计值
wXX1=zeros(6,1,N4);
PP1=zeros(6,6,N4);%存放预测滤波协方差矩阵
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);%存放估计值
  wXX=zeros(6,1,N4);
 PP=zeros(6,6,N4);%存放滤波协方差矩阵
 %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]';%初始状态值估计
 XX(:,:,3)=[403 295 0 3602 370 0];
 wXX(:,:,3)=[403 295 0 3602 370 0];
%对PP(:,:,3)进行初始化

PP(:,:,3)=[1 0 0 0 0 0;0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%KALMAN 滤波1%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i=3:N4-1
    Q11(:,:,i)=2*al*a2x(i)*q2;%计算Q矩阵
    Q22(:,:,i)=2*al*a2y(i)*q2;%计算Q矩阵
    Q(:,:,i)=[Q11(:,:,i) Q12(:,:,i);Q12(:,:,i) Q22(:,:,i)];%计算Q矩阵
    XX1(:,:,i)=Qb*XX(:,:,i);%计算一步预测估计值                         X(k/k-1)=Q(k+1,k)X(k-1/k-1)
    PP1(:,:,i)=Qb*PP(:,:,i)*Qb'+Q(:,:,i);%计算预测滤波协方差矩阵         P(k/k-1)=Q(k+1,k)P(k-1/k-1)Q'(k+1,k)+G(k-1)Q(k-1)G'(k-1) %G(k-1)=I%
    K(:,:,i)=PP1(:,:,i)*H'*inv(H*PP1(:,:,i)*H'+R(:,:,i));%计算增益矩阵  K(k)=P(k/k-1)H(k)'[H(k)P(k/k-1)H(k)'+R(k)]"
    XX(:,:,i+1)=XX1(:,:,i)+K(:,:,i)*(Z(:,:,i+1)-H*XX1(:,:,i));%计算估计值
    PP(:,:,i+1)=(I-K(:,:,i+1)*H)*PP1(:,:,i);%计算协方差更新方程          P(k/k)=[I-K(k)H(k)]P(k/k-1)
end
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%KALMAN 滤波2%%%%%%%%%%%%%%
for i=3:N4-1
    Q11(:,:,i)=2*al*a2x(i)*q2;%计算Q矩阵
    Q22(:,:,i)=2*al*a2y(i)*q2;%计算Q矩阵
    Q(:,:,i)=[Q11(:,:,i) Q12(:,:,i);Q12(:,:,i) Q22(:,:,i)];%计算Q矩阵
    wXX1(:,:,i)=Qb*wXX(:,:,i);%计算一步预测估计值                         X(k/k-1)=Q(k+1,k)X(k-1/k-1)
    PP1(:,:,i)=Qb*PP(:,:,i)*Qb'+Q(:,:,i);%计算预测滤波协方差矩阵         P(k/k-1)=Q(k+1,k)P(k-1/k-1)Q'(k+1,k)+G(k-1)Q(k-1)G'(k-1) %G(k-1)=I%
    K(:,:,i)=PP1(:,:,i)*H'*inv(H*PP1(:,:,i)*H'+R(:,:,i));%计算增益矩阵  K(k)=P(k/k-1)H(k)'[H(k)P(k/k-1)H(k)'+R(k)]"
    wXX(:,:,i+1)=wXX1(:,:,i)+K(:,:,i)*(w(:,:,i+1)-H*wXX1(:,:,i));%计算估计值
    PP(:,:,i+1)=(I-K(:,:,i+1)*H)*PP1(:,:,i);%计算协方差更新方程          P(k/k)=[I-K(k)H(k)]P(k/k-1)
end






%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%图形显示%%%%%%%%%%%%%%%%%%%%%%%%
x_filter=zeros(1,N4);
y_filter=zeros(1,N4);

wx_filter=zeros(1,N4);
wy_filter=zeros(1,N4);

for k=1:N4;
x_filter(k)=XX(1,1,k);
y_filter(k)=XX(4,1,k);
end

for k=1:N4;
wx_filter(k)=wXX(1,1,k);
wy_filter(k)=wXX(4,1,k);
end
figure(2)
subplot(2,1,1)
 plot(sx,sy,'r-',guance_x,guance_y,'b-x');
 hold on
 plot(x_filter,y_filter,'g-h');
 legend('真实飞行轨迹','观测飞行轨迹','估计飞行轨迹',2);
 title('目标在空间中的真实飞行轨迹与估计轨迹')
 subplot(2,1,2)
  plot(sx,sy,'r-',xA3,yA3,'b-x');
 hold on
 plot(wx_filter,wy_filter,'g-h');
 legend('真实飞行轨迹','观测飞行轨迹','估计飞行轨迹',2);
 title('目标在空间中的真实飞行轨迹与使用小波后的估计轨迹——db2,vk(1,1,i)=10*e2(i) ')