gpb_2.m

广义伪贝叶斯算法

%**************************************************************************
%                      GPB 2 Multiple Model Approach 
% *************************************************************************

%CV(second order ,without process noise)----model 1;
%Wiener process acceleration(third order model)----model 2;
%在二维平面上，航线恒定，速度恒定，在400秒--600秒时进行90度的慢转弯,610秒到660秒完成90度快速转弯
%测量值只有位置


clear all;
close all;
% define the sample interval
delta=10;                   % s   
%define the number of samples
nsample=100;
%define the number of of simulation loops
nloops=100;
%define process noise intensity
q2=0.0316^2;
%define measurement  noise intensity
r=10^4;     %m^2
%the slow turn acceleration inputs  400s<=t<=600s
xa=0.075;
ya=0.075;       %m/s^2
%the fast turn acceleration inputs  610s<=t<=660s
xa2=-0.3;
ya2=-0.3;     %m/s^2

T1=40;
T2=61;
T3=66;


nx1=4;
nx2=6
nz=2;

tic


     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
     %      CV(second order with no process noise )%%
     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
     
     %define F matrix (transition matrix)for CV model
     F1=[1,delta,0,0;
           0,1,0,0;
           0,0,1,delta;
           0,0,0,1];
             %define H matrix(measurement matrix)
     H1=[1,0,0,0;
         0,0,1,0];
     
     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
     %    Wienner process acceleration model(third order) %%
     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
       % define F matrix for third order model 
        F2=[1 delta 0 0  delta^2/2 0;
            0 1 0 0 delta 0;
            0 0 1 delta 0 delta^2/2;
            0 0 0 1 0 delta;
            0 0 0 0 1 0;
            0 0 0 0 0 1];
        G2=[delta^2/2 0;
             delta 0;
             0 delta^2/2;
             0 delta;
             1 0;
              0 1];
    
    
      %define H matrix(measurement matrix)

        H2=[1,0,0,0,0,0;
        0,0,1,0,0,0];

     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %                observation calculation              %
     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   
        
        % 生成真实航迹
        xtr=zeros(nx1,nsample);
        xtr(:,1)=[2000,0,9850,-15]'; %初始值
       for i=2:T1
          xtr(:,i)=F1*xtr(:,i-1);
       end
      for i=(T1+1):60
         xtr(1,i)=xtr(1,i-1)+xtr(2,i-1)*delta+0.5*xa*delta^2;
         xtr(2,i)=xtr(2,i-1)+xa*delta;
         xtr(3,i)=xtr(3,i-1)+xtr(4,i-1)*delta+0.5*ya*delta^2;
         xtr(4,i)=xtr(4,i-1)+ya*delta;
      end
     xtr(1,61)=xtr(1,60)+xtr(2,60)*delta;
     xtr(2,61)=xtr(2,60);
     xtr(3,61)=xtr(3,60)+xtr(4,60)*delta;
     xtr(4,61)=xtr(4,60);
     
     for i=(T2+1):T3
         xtr(2,i)=xtr(2,i-1)+xa2*delta;
         xtr(1,i)=xtr(1,i-1)+xtr(2,i-1)*delta+0.5*xa2*delta^2;
         xtr(4,i)=xtr(4,i-1)+ya2*delta;
         xtr(3,i)=xtr(3,i-1)+xtr(4,i-1)*delta+0.5*ya2*delta^2;
   
     end  
     for i=(T3+1):nsample
         xtr(:,i)=F1*xtr(:,i-1);
     end  




 %define empty output matries
     xposition=zeros(1,nsample);   
%      xvelocity=zeros(1,nsample);    
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  loop over the target motion /mearurment simulation                %%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 for kk=1:nloops
%    tic
    kk;

      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      %%%  产生观测 ，仅仅是距离                     %%%
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
        wx=sqrt(r)*randn(1,nsample);%x方向的观测误差
        wy=sqrt(r)*randn(1,nsample);%y方向的观测误差
        zob(1,1:nsample)=xtr(1,:)+wx;
        zob(2,1:nsample)=xtr(3,:)+wy;

      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      %  initialization process         
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      
       xhat_1=zeros(nx1,nsample);                  %状态滤波矩阵初始化
       xhat_2=zeros(nx2,nsample);   
       xhat_11=zeros(nx1,nsample); 
       xhat_12=zeros(nx2,nsample); 
       xhat_21=zeros(nx1,nsample); 
       xhat_22=zeros(nx2,nsample); 
       xpre_11=zeros(nx1,nsample);
       xpre_12=zeros(nx2,nsample);
       xpre_21=zeros(nx1,nsample);
       xpre_22=zeros(nx2,nsample);
       
       Phat_1=zeros(nx1,nx1,nsample);  
       Phat_2=zeros(nx2,nx2,nsample);  
       Phat_11=zeros(nx1,nx1,nsample);  
       Phat_12=zeros(nx2,nx2,nsample);  
       Phat_21=zeros(nx1,nx1,nsample);  
       Phat_22=zeros(nx2,nx2,nsample); 
       Ppre_11=zeros(nx1,nx1,nsample);            %预计协方差
       Ppre_12=zeros(nx2,nx2,nsample);            %预计协方差
       Ppre_21=zeros(nx1,nx1,nsample);            %预计协方差
       Ppre_22=zeros(nx2,nx2,nsample);            %预计协方差
      
           
    
       ztilde_11=zeros(nz,nsample);                  %新息矩阵初始化
       ztilde_12=zeros(nz,nsample);   
       ztilde_21=zeros(nz,nsample);   
       ztilde_22=zeros(nz,nsample);   
       
       S11=zeros(nz,nz,nsample);                 %新息方差矩阵初始化
       S12=zeros(nz,nz,nsample);                 %新息方差矩阵初始化
       S21=zeros(nz,nz,nsample); 
       S22=zeros(nz,nz,nsample); 
       K11=zeros(nx1,nz,nsample);                   % kalman filter gain
       K12=zeros(nx2,nz,nsample);
       K21=zeros(nx1,nz,nsample); 
       K22=zeros(nx2,nz,nsample); 
       
       zpre_11=zeros(nz,nsample);                  %测量值预测值
       zpre_12=zeros(nz,nsample); 
       zpre_21=zeros(nz,nsample);  
       zpre_22=zeros(nz,nsample);  
       
       xerror=zeros(4,nsample);
       xout=zeros(4,nsample);
       Pout=zeros(4,4,nsample);
       
       mu=zeros(2,nsample);
       mu_11=zeros(1,nsample);
       mu_12=zeros(1,nsample);
       mu_21=zeros(1,nsample);
       mu_22=zeros(1,nsample);
       
       xhat_1(:,2)=[zob(1,2) (zob(1,2)-zob(1,1))/delta  zob(2,2) (zob(2,2)-zob(2,1))/delta]';
         r1=[r r/delta;r/delta 2*r/delta^2];
         r2=[r/delta^2 0;2*r/delta^3 0];
         r3=[0 r/delta^2;0 2*r/delta^3];
          r4=[2*r/delta^4 0;0 2*r/delta^4];
       Phat_1(:,:,2)=[r1,zeros(2);zeros(2),r1];

       xhat_2(:,2)=[zob(1,2) (zob(1,2)-zob(1,1))/delta  zob(2,2) (zob(2,2)-zob(2,1))/delta 2*(zob(1,2)-zob(1,1))/delta^2 2*(zob(2,2)-zob(2,1)+150)/delta^2]';
       Phat_2(:,:,2)=[r1 zeros(2) r2;zeros(2) r1 r3;r2' r3' r4];
    
   
    
       %initial model probabilities for state
    
        mu(:,2)=[0.5;
                 0.5];
     
       %probabilities of changing state,Markove chain transition
       rho=[0.95,0.05;0.05,0.95];
       rho11=rho(1,1);
       rho12=rho(1,2);
       rho21=rho(2,1);
       rho22=rho(2,2);
      
       
       
       
       
       
   
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %                             kalman filter
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

      for ii=3:nsample
       
          
          
          %  CV model 滤波
          xpre_11(:,ii)=F1*xhat_1(:,ii-1);
          Ppre_11(:,:,ii)=F1*Phat_1(:,:,ii-1)*F1';
          zpre_11(:,ii)=H1*xpre_11(:,ii);
          ztilde_11(:,ii)=zob(:,ii)-zpre_11(:,ii);
          S11(:,:,ii)=H1*Ppre_11(:,:,ii)*H1'+r*eye(2);
          K11(:,:,ii)=Ppre_11(:,:,ii)*H1'*inv(S11(:,:,ii));
          xhat_11(:,ii)=xpre_11(:,ii)+K11(:,:,ii)*ztilde_11(:,ii);
          Phat_11(:,:,ii)=Ppre_11(:,:,ii)-K11(:,:,ii)*S11(:,:,ii)*K11(:,:,ii)';
          
                
          lamda_11=exp(-0.5*ztilde_11(:,ii)'*inv(S11(:,:,ii))*ztilde_11(:,ii))/sqrt(det(2*pi*S11(:,:,ii)));
          
          xpre_21(:,ii)=F1*xhat_2(1:4,ii-1);
          Ppre_21(:,:,ii)=F1*Phat_2(1:4,1:4,ii-1)*F1';
          zpre_21(:,ii)=H1*xpre_21(:,ii);
          ztilde_21(:,ii)=zob(:,ii)-zpre_21(:,ii);
          S21(:,:,ii)=H1*Ppre_21(:,:,ii)*H1'+r*eye(2);
          K21(:,:,ii)=Ppre_21(:,:,ii)*H1'*inv(S21(:,:,ii));
          xhat_21(:,ii)=xpre_21(:,ii)+K21(:,:,ii)*ztilde_21(:,ii);
          Phat_21(:,:,ii)=Ppre_21(:,:,ii)-K21(:,:,ii)*S21(:,:,ii)*K21(:,:,ii)';
          
                
          lamda_21=exp(-0.5*ztilde_21(:,ii)'*inv(S21(:,:,ii))*ztilde_21(:,ii))/sqrt(det(2*pi*S21(:,:,ii)));
          
          
          % W A model 滤波
          xpre_12(:,ii)=F2*[xhat_1(:,ii-1);0;0];
          Ppre_12(:,:,ii)=F2*[Phat_1(:,:,ii-1),zeros(4,2);zeros(2,6)]*F2'+G2*q2*eye(2)*G2';
          zpre_12(:,ii)=H2*xpre_12(:,ii);
          ztilde_12(:,ii)=zob(:,ii)-zpre_12(:,ii);
          S12(:,:,ii)=H2*Ppre_12(:,:,ii)*H2'+r*eye(2);
          K12(:,:,ii)=Ppre_12(:,:,ii)*H2'*inv(S12(:,:,ii));
          xhat_12(:,ii)=xpre_12(:,ii)+K12(:,:,ii)*ztilde_12(:,ii);
          Phat_12(:,:,ii)=Ppre_12(:,:,ii)-K12(:,:,ii)*S12(:,:,ii)*K12(:,:,ii)';
          
        
          lamda_12=exp(-0.5*ztilde_12(:,ii)'*inv(S12(:,:,ii))*ztilde_12(:,ii))/sqrt(det(2*pi*S12(:,:,ii)));
          
          xpre_22(:,ii)=F2*xhat_2(:,ii-1);
          Ppre_22(:,:,ii)=F2*Phat_2(:,:,ii-1)*F2'+G2*q2*eye(2)*G2';
          zpre_22(:,ii)=H2*xpre_22(:,ii);
          ztilde_22(:,ii)=zob(:,ii)-zpre_22(:,ii);
          S22(:,:,ii)=H2*Ppre_22(:,:,ii)*H2'+r*eye(2);
          K22(:,:,ii)=Ppre_22(:,:,ii)*H2'*inv(S22(:,:,ii));
          xhat_22(:,ii)=xpre_22(:,ii)+K22(:,:,ii)*ztilde_22(:,ii);
          Phat_22(:,:,ii)=Ppre_22(:,:,ii)-K22(:,:,ii)*S22(:,:,ii)*K22(:,:,ii)';
          
        
          lamda_22=exp(-0.5*ztilde_22(:,ii)'*inv(S22(:,:,ii))*ztilde_22(:,ii))/sqrt(det(2*pi*S22(:,:,ii)));
          
          
          %计算模型混合概率
           c_1=lamda_11*rho11*mu(1,ii-1)+lamda_21*rho21*mu(2,ii-1);
            c_2=lamda_12*rho12*mu(1,ii-1)+lamda_22*rho22*mu(2,ii-1);
           mu_11(ii)=lamda_11*rho11*mu(1,ii-1)/c_1;
           mu_12(ii)=lamda_12*rho12*mu(1,ii-1)/c_2;
           mu_21(ii)=lamda_21*rho21*mu(2,ii-1)/c_1;
           mu_22(ii)=lamda_22*rho22*mu(2,ii-1)/c_2;
         
          %Merging
          
          xhat_1(:,ii)=mu_11(ii)*xhat_11(:,ii)+mu_21(ii)*xhat_21(:,ii);
          xhat_2(:,ii)=mu_12(ii)*xhat_12(:,ii)+mu_22(ii)*xhat_22(:,ii);
          Phat_1(:,:,ii)=mu_11(ii)*(Phat_11(:,:,ii)+(xhat_11(:,ii)-xhat_1(:,ii))*(xhat_11(:,ii)-xhat_1(:,ii))')+...
                         mu_21(ii)*(Phat_21(:,:,ii)+(xhat_21(:,ii)-xhat_1(:,ii))*(xhat_21(:,ii)-xhat_1(:,ii))');
          Phat_2(:,:,ii)=mu_12(ii)*(Phat_12(:,:,ii)+(xhat_12(:,ii)-xhat_2(:,ii))*(xhat_12(:,ii)-xhat_2(:,ii))')+...
                         mu_22(ii)*(Phat_22(:,:,ii)+(xhat_22(:,ii)-xhat_2(:,ii))*(xhat_22(:,ii)-xhat_2(:,ii))');
                     
                         
          % Mode probability updating 
          
          c=c_1+c_2;
          mu(1,ii)=c_1/c;
          mu(2,ii)=c_2/c;
          
          %State estimate and covariance combination
          
          xout(:,ii)=xhat_1(:,ii)*mu(1,ii)+xhat_2(1:4,ii)*mu(2,ii);
          Pout(:,:,ii)=mu(1,ii)*(Phat_1(:,:,ii)+(xhat_1(:,ii)-xout(:,ii))*(xhat_1(:,ii)-xout(:,ii))')+...
                       mu(2,ii)*(Phat_2(1:4,1:4,ii)+(xhat_2(1:4,ii)-xout(:,ii))*(xhat_2(1:4,ii)-xout(:,ii))');
                   
                 
         xerror(:,ii)=xtr(:,ii)-xout(:,ii);
      
      end   %ii=2:nsample
       
   for k=3:nsample
    xposition(k)=xposition(k)+xerror(1,k)^2;
%     xvelocity(k)=xvelocity(k)+xerror(2,k)^2;

   end
%    toc
 end  %1:nloops

 toc
 
for k=3:nsample
xposition(k)=sqrt(xposition(k)/nloops);
% xvelocity(k)=sqrt(xvelocity(k)/nloops);

end
figure(1)
subplot(211)
plot(xtr(1,3:nsample),xtr(3,3:nsample),'r-',xout(1,3:nsample),xout(3,3:nsample),'b--',zob(1,3:nsample),zob(2,3:nsample),'g:');
xlabel('X');
ylabel('Y');
legend('-真实轨迹','--GPB2估计轨迹',':观测轨迹');
title('real line and estimation');


subplot(212)

plot(xposition(1,3:nsample));
hold on;
plot(1:100,100*ones(1,100),'r-');
title('x position RMS error for GPB 2');
%  subplot(3,1,2),plot(xvelocity(1,3:nsample));
%  title('Velocity RMS error for IMM2');
%  subplot(313),plot(mu(2,3:100));
% title('Maneuvering mode probability in IMM2')
 figure(3)