gpb_1.m

广义伪贝叶斯算法

%**************************************************************************
%                      GPB 1 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

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:40
    xtr(:,i)=F1*xtr(:,i-1);
end
for i=41: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=62:66
    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=67:100
    xtr(:,i)=F1*xtr(:,i-1);
end  



% figure(2);
% subplot(211),plot(1:100,xtr(1,1:100));
% subplot(212),plot(1:100,xtr(3,:));

 %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);   
       xpre_1=zeros(nx1,nsample);
       xpre_2=zeros(nx2,nsample);
       
       
       
       Phat_1=zeros(nx1,nx1,nsample);  
       Phat_2=zeros(nx2,nx2,nsample);  
       Ppre_1=zeros(nx1,nx1,nsample);            %预计协方差
       Ppre_2=zeros(nx2,nx2,nsample);            %预计协方差
               
    
       ztilde_1=zeros(nz,nsample);                  %新息矩阵初始化
       ztilde_2=zeros(nz,nsample);   
            
       S1=zeros(nz,nz,nsample);                 %新息方差矩阵初始化
       S2=zeros(nz,nz,nsample);                 %新息方差矩阵初始化
       K1=zeros(nx1,nz,nsample);                   % kalman filter gain
       K2=zeros(nx2,nz,nsample);
         
       zpre_1=zeros(nz,nsample);                  %测量值预测值
       zpre_2=zeros(nz,nsample); 
   
       xerror=zeros(4,nsample);
       xhat=zeros(4,nsample);
       Phat=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(:,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];
      
     Phat(:,:,2)=[r1,zeros(2);zeros(2),r1];
     
       %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
       
          
          
          %   滤波器1滤波
          
          xpre_1(:,ii)=F1*xhat(:,ii-1);
          Ppre_1(:,:,ii)=F1*Phat(:,:,ii-1)*F1';
          zpre_1(:,ii)=H1*xpre_1(:,ii);
          ztilde_1(:,ii)=zob(:,ii)-zpre_1(:,ii);
          S1(:,:,ii)=H1*Ppre_1(:,:,ii)*H1'+r*eye(2);
          K1(:,:,ii)=Ppre_1(:,:,ii)*H1'*inv(S1(:,:,ii));
          xhat_1(:,ii)=xpre_1(:,ii)+K1(:,:,ii)*ztilde_1(:,ii);
          Phat_1(:,:,ii)=Ppre_1(:,:,ii)-K1(:,:,ii)*S1(:,:,ii)*K1(:,:,ii)';
          
                
          lamda_1=exp(-0.5*ztilde_1(:,ii)'*inv(S1(:,:,ii))*ztilde_1(:,ii))/sqrt(det(2*pi*S1(:,:,ii)));
          
          
          
          % 滤波器2滤波
          
          xpre_2(:,ii)=F2*[xhat(:,ii-1);0;0];
          Ppre_2(:,:,ii)=F2*[Phat(:,:,ii-1),zeros(4,2);zeros(2,6)]*F2'+G2*q2*eye(2)*G2';
          zpre_2(:,ii)=H2*xpre_2(:,ii);
          ztilde_2(:,ii)=zob(:,ii)-zpre_2(:,ii);
          S2(:,:,ii)=H2*Ppre_2(:,:,ii)*H2'+r*eye(2);
          K2(:,:,ii)=Ppre_2(:,:,ii)*H2'*inv(S2(:,:,ii));
          xhat_2(:,ii)=xpre_2(:,ii)+K2(:,:,ii)*ztilde_2(:,ii);
          Phat_2(:,:,ii)=Ppre_2(:,:,ii)-K2(:,:,ii)*S2(:,:,ii)*K2(:,:,ii)';
          
        
          lamda_2=exp(-0.5*ztilde_2(:,ii)'*inv(S2(:,:,ii))*ztilde_2(:,ii))/sqrt(det(2*pi*S2(:,:,ii)));
          
                  
          % Mode probability updating 
          
         c=lamda_1*(rho11*mu(1,ii-1)+rho21*mu(2,ii-1))+lamda_2*(rho12*mu(1,ii-1)+rho22*mu(2,ii-1));
         mu(1,ii)=lamda_1*(rho11*mu(1,ii-1)+rho21*mu(2,ii-1))/c;
         mu(2,ii)=lamda_2*(rho12*mu(1,ii-1)+rho22*mu(2,ii-1))/c;
         
        
          
          %State estimate and covariance combination
          
          xhat(:,ii)=xhat_1(:,ii)*mu(1,ii)+xhat_2(1:4,ii)*mu(2,ii);
          Phat(:,:,ii)=mu(1,ii)*(Phat_1(:,:,ii)+(xhat_1(:,ii)-xhat(:,ii))*(xhat_1(:,ii)-xhat(:,ii))')+...
                       mu(2,ii)*(Phat_2(1:4,1:4,ii)+(xhat_2(1:4,ii)-xhat(:,ii))*(xhat_2(1:4,ii)-xhat(:,ii))');
                   
                 
         xerror(:,ii)=xtr(:,ii)-xhat(:,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(2)
subplot(211)
plot(xtr(1,3:nsample),xtr(3,3:nsample),'r-',xhat(1,3:nsample),xhat(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-');
xlabel('TIME STEP');
ylabel('x position RMS error');
title('x position RMS error for GPB 1');