myalgorithm.m

关于粒子滤波的仿真程序,比较了粒子滤波和卡尔曼滤波的优缺点

function [StdErr] = myalgorithm
 
 N=1000; % number of particles.
 tf=50; % simulation length
 
 xsensor=0;
 ysensor=0; 
 s=[xsensor ysensor]; %传感器位置
 
 x=[0;0;1;1]; % initial state
 xrho=x(1,:);
 yrho=x(2,:);
 rho=[xrho yrho]; %目标位置
 xvtarget = x(3,:);
 yvtarget = x(4,:);
 vtarget = [xvtarget yvtarget]; %目标速度
 atarget=2;
 r=500; % radius of target
 
 Px=diag([25 25 1 1]); % initial estimation error covariance
 Q=[1 0;0 1]; % process noise covariance
 R=1; % measurement noise variance
 Ts=1;
 
 P0=30;
 d0=1;
 alpha=2.3;
 theta=pi*60/180;
 
 L=7;
 belta=0;
 ki=2;
 lambda=alpha^2*(L+ki)-L;
   
 xhat=x; %intial state estimate
 
%  Initialize the particle filter.
 for i=1:N
     xpart(:,i) = x + sqrt(Px) * [randn;randn;randn;randn]; % standard particle filter
 end

%  xArr=x;
%  xhatArr=xhat;
%  xhatUKFArr=xhat;
 
 for k=1:tf
     % System simulation
     xmsensor(k)=xrho(k)-r*cos(theta);
     ymsensor(k)=yrho(k)-r*sin(theta); 
     
     xrho(k+1)=xrho(k)+xvtarget*Ts;
     yrho(k+1)=yrho(k)+yvtarget*Ts;
     
     u(k)=atan(((xsensor(k)-xmsensor(k))^2+((ysensor(k)-ymsensor(k))^2)/2));

     xsensor(k+1)=xsensor(k)+u(k)*Ts;
     ysensor(k+1)=ysensor(k)+u(k)*Ts;
     
     x = kron([1 Ts;0 1],eye(2,2))*x + kron([Ts^2/2;Ts],eye(2,2))*sqrt(Q)*[randn;randn];
     y = P0-10*alpha*log10(sqrt((xsensor(k)-x(1,:))^2 + (ysensor(k)-x(2,:))^2)/d0) + sqrt(R) * randn;

     % Simulate the continuous-time part of the particle filters(time update)
     for i=1:N
         % standerd particle filter
         xpartminus(:,i) = kron([1 Ts;0 1],eye(2,2))*xpart(:,i) + kron([Ts^2/2;Ts],eye(2,2))*sqrt(Q)*[randn;randn];
         ypart=P0-10*alpha*log10(sqrt((xsensor(k)-xpartminus(1,i))^2+(ysensor(k)-xpartminus(2,i))^2)/d0) ;
         
         vhat(i)=y-ypart; %观测和预测的差
     end
     
     % Normalize the likelihood of each a priori eatimate
     vhatscale = max(abs(vhat))/4;
     qsum = 0;
     for i = 1:N
         q(i)=exp(-(vhat(i)/vhatscale)^2);
         qsum = qsum+q(i);
     end
     % Normalize the likelihood of each a priori estimate.
     for i = 1:N
         q(i)=q(i)/qsum;
     end
     % Resample the standard particle filter
    for i = 1 : N
        u = rand; % uniform random number between 0 and 1
        qtempsum = 0;
        for j = 1 : N
            qtempsum = qtempsum + q(j);
            if qtempsum >= u
                xpart(:,i) = xpartminus(:,j);
                break;
            end
        end
    end
    % The standard particle filter estimate is the mean of the particles.
    
    xhat(:,k) = mean(xpart')';

    % Save data in arrays for later plotting
%     xArr=[xArr x];
%     xhatArr=[xhatArr xhat];
 end
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %%%%%%%%%%%%%%%%%%%%%%% Unscented Particle Filter %%%%%%%%%%%%%%%%%%%%%%%%
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 N=1000; % number of particles.
 tf=50; % simulation length
 
 xsensor=0;
 ysensor=0; 
 s=[xsensor ysensor]; %传感器位置
 
 x=[0;0;1;1]; % initial state
 xrho=x(1,:);
 yrho=x(2,:);
 rho=[xrho yrho]; %目标位置
 xvtarget = x(3,:);
 yvtarget = x(4,:);
 vtarget = [xvtarget yvtarget]; %目标速度
 atarget=2;
 r=500; % radius of target
 
 Px=diag([25 25 1 1]); % initial estimation error covariance
 Q=[1 0;0 1]; % process noise covariance
 R=1; % measurement noise variance
 Ts=1;
 
 P0=30;
 d0=1;
 alpha=2.3;
 theta=pi*60/180;
 
 L=7;
 belta=0;
 ki=2;
 lambda=alpha^2*(L+ki)-L;
 
 Anoise=zeros(4,tf);
 X_sigma=zeros(7,tf);  
 P_sigma=zeros(7,7,tf);
 % Initialization
%  for i = 1:N
     xestimate = x;
     P = Px;
     X_sigma(:,1) = [(xestimate)' 0 0 0]';
     P_sigma(:,:,1) = blkdiag(P,Q,R);  
%  end
 
 for k=1:tf
     % System simulation
     xmsensor(k)=xrho(k)-r*cos(theta);
     ymsensor(k)=yrho(k)-r*sin(theta); 
     
     xrho(k+1)=xrho(k)+xvtarget*Ts;
     yrho(k+1)=yrho(k)+yvtarget*Ts;
     
     u(k)=atan(((xsensor(k)-xmsensor(k))^2+((ysensor(k)-ymsensor(k))^2)/2));

     xsensor(k+1)=xsensor(k)+u(k)*Ts;
     ysensor(k+1)=ysensor(k)+u(k)*Ts;
     
     x = kron([1 Ts;0 1],eye(2,2))*x + kron([Ts^2/2;Ts],eye(2,2))*sqrt(Q)*[randn;randn];
     y = P0-10*alpha*log10(sqrt((xsensor(k)-x(1,:))^2 + (ysensor(k)-x(2,:))^2)/d0) + sqrt(R) * randn;

     % Simulate the continuous-time part of the particle filters(time update)
     for i=1:N
         % update the particles with UKF
         % Caculate sigma points:
         for j = 1 : 2*L+1
             Wm(j) = 1/(2*(L+lambda));
             Wc(j) = 1/(2*(L+lambda)); % 权重
         end
         
         Wm(1) = lambda/(L+lambda);
         Wc(1) = Wm(1)+(1-alpha^2+belta); % 权值
         
         cho = (chol(P_sigma(:,:,k)*(L+lambda)))';
        
         for j = 1:L
             XsigmaP1(:,j) = X_sigma(:,k)+cho(:,j);
             XsigmaP2(:,j) = X_sigma(:,k)-cho(:,j);
         end
         Xsigma = [X_sigma(:,k) XsigmaP1 XsigmaP2];
         
         % Propagate particle into future(time update)
         Xsigmapre(1:4,:) = kron([1 Ts;0 1],eye(2,2))*Xsigma(1:4,:) + kron([Ts^2/2;Ts],eye(2,2))*sqrt(Q)*Xsigma(5:6,:);
         xpred = zeros(4,1);
         for j = 1:2*L+1
             xpred = xpred+Wm(j)*Xsigmapre(1:4,j);
         end
         ppred = zeros(4,4);
         for j = 1:2*L+1
             ppred = ppred+Wc(j)*(Xsigmapre(1:4,j)-xpred)*(Xsigmapre(1:4,j)-xpred)';
         end
         
         xestimate2 = [(xpred)' 0 0 0]';
         ppred2 = blkdiag(ppred,Q,R);
         
         chor = (chol((L+lambda)*ppred2))';
         for j = 1:L
             XaugsigmaP1(:,j) = xestimate2+chor(:,j);
             XaugsigmaP2(:,j) = xestimate2-chor(:,j);
         end
         Xaugsigma = [xestimate2 XaugsigmaP1 XaugsigmaP2];
         
         for j = 1:2*L+1
             Ysigmapre(j) = P0-10*alpha*log10(sqrt((xsensor(k)-Xaugsigma(1,j))^2+(ysensor(k)-Xaugsigma(2,j))^2)/d0)+sqrt(R)*Xsigma(7,j);
         end
         
         ypred = zeros(1,1);
         for j = 1:2*L+1
             ypred = ypred+Wm(j)*Ysigmapre(j);
         end
         
         % incorporate new observation(measure update)
         Pyy = zeros(1,1);
         Pxy = zeros(4,1);
         for j = 1:2*L+1
             Pyy = Pyy+Wc(j)*(Ysigmapre(j)-ypred)*(Ysigmapre(j)-ypred)';
             Pxy = Pxy+Wc(j)*(Xsigmapre(1:4,j)-xpred)*(Ysigmapre(j)-ypred)';
         end
         
         K=Pxy*pinv(Pyy);
         xestimate=xpred+K*(y-ypred);
         P = ppred - K*Pyy*K';
         
         X_sigma(:,k+1) = [(xestimate)' 0 0 0]';
         P_sigma(:,:,k+1) = blkdiag(P,Q,R);  
     
%          Anoise(:,k)=xestimate;
     
         xUKFpart(:,i) = xestimate+sqrt(P)*[randn;randn;randn;randn];
         
         % UKF particle filter
        xUKFpartminus(:,i) = xUKFpart(:,i);
%           xUKFpartminus(:,i) = kron([1 Ts;0 1],eye(2,2))*xUKFpart(:,i) + kron([Ts^2/2;Ts],eye(2,2))*sqrt(Q)*[randn;randn];
         yUKFpart=P0-10*alpha*log10(sqrt((xsensor(k)-xUKFpartminus(1,i))^2+(ysensor(k)-xUKFpartminus(2,i))^2)/d0) ;
         
        vhatUKF(i) = y-yUKFpart;
              
%         vhatUKF(i) = xestimate-xUKFpart(:,i);
     end
     
     % Normalize the likelihood of each a priori eatimate
     vhatscaleUKF = max(abs(vhatUKF))/4;
     qsumUKF = 0;
     for i = 1:N
         qUKF(i)=exp(-(vhatUKF(i)/vhatscaleUKF)^2);
         qsumUKF = qsumUKF+qUKF(i);
     end
     % Normalize the likelihood of each a priori estimate.
     for i = 1:N
         qUKF(i)=qUKF(i)/qsumUKF;
     end
     % Resample the standard particle filter
    for i = 1 : N
        u = rand; % uniform random number between 0 and 1
        qtempsum = 0;
        for j = 1 : N
            qtempsum = qtempsum + qUKF(j);
            if qtempsum >= u
                xUKFpart(:,i) = xUKFpartminus(:,j);
                break;
            end
        end
    end
    % The standard particle filter estimate is the mean of the particles.
    
    xhatUKF(:,k) = mean(xUKFpart')';

    % Save data in arrays for later plotting
%     xhatUKFArr=[xhatUKFArr xhatUKF];
end

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 close all;
 t = 0 : tf;
% figure;
% plot(Anoise(1,:),Anoise(2,:),'b-');
% plot(Xsigma,'b-');
% plot(xrho, yrho,'b-');
% figure;
% plot(xsensor, ysensor,'b-');
% 
% figure;
% plot(1:tf,y,'r-');
% figure;
% plot(1:100,ypart,'r-');
% figure;
% plot(1:100,vhat,'r-');

figure;
plot(xrho, yrho,'k-');hold on;
plot(xhat(1,:),xhat(2,:),'r-');hold on;
plot(xhatUKF(1,:),xhatUKF(2,:),'b-');
%  
% for i=1:4
%     StdErr(i)=sqrt((norm(xArr(i,:)-xhatArr(i,:)))^2/tf);
% end
% disp(['Standaed Particle filter RMS error=','num2str(StdErr)']);
% plot(StdErr,'b-');