www.pudn.com > particlefilter.rar > ParticleEx4.m, change:2007-01-11,size:7676b


function [StdRMSErr, AuxRMSErr] = ParticleEx4 
 
% Particle filter example. 
% Track a body falling through the atmosphere. 
% This system is taken from [Jul00], which was based on [Ath68]. 
% Compare the particle filter with the auxiliary particle filter. 
 
global rho0 g k dt 
 
rho0 = 2; % lb-sec^2/ft^4 
g = 32.2; % ft/sec^2 
k = 2e4; % ft 
R = 10^4; % measurement noise variance (ft^2) 
Q = diag([0 0 0]); % process noise covariance 
M = 10^5; % horizontal range of position sensor 
a = 10^5; % altitude of position sensor 
P = diag([1e6 4e6 10]); % initial estimation error covariance 
 
x = [3e5; -2e4; 1e-3]; % initial state 
xhat = [3e5; -2e4; 1e-3]; % initial state estimate 
 
N = 200; % number of particles   
 
% Initialize the particle filter. 
for i = 1 : N 
    xhatplus(:,i) = x + sqrt(P) * [randn; randn; randn]; % standard particle filter 
    xhatplusAux(:,i) = xhatplus(:,i); % auxiliary particle filter 
end 
 
T = 0.5; % measurement time step 
randn('state',sum(100*clock)); % random number generator seed 
 
tf = 30; % simulation length (seconds) 
dt = 0.5; % time step for integration (seconds) 
xArray = x; 
xhatArray = xhat; 
xhatAuxArray = xhat; 
 
for t = T : T : tf 
    fprintf('.'); 
    % Simulate the system. 
    for tau = dt : dt : T 
        % Fourth order Runge Kutta ingegration 
        [dx1, dx2, dx3, dx4] = RungeKutta(x); 
        x = x + (dx1 + 2 * dx2 + 2 * dx3 + dx4) / 6; 
        x = x + sqrt(dt * Q) * [randn; randn; randn] * dt; 
    end 
    % Simulate the noisy measurement. 
    z = sqrt(M^2 + (x(1)-a)^2) + sqrt(R) * randn; 
    % Simulate the continuous-time part of the particle filter (time update). 
    xhatminus = xhatplus; 
    xhatminusAux = xhatplusAux; 
    for i = 1 : N 
        for tau = dt : dt : T 
            % Fourth order Runge Kutta ingegration 
            % standard particle filter 
            [dx1, dx2, dx3, dx4] = RungeKutta(xhatminus(:,i)); 
            xhatminus(:,i) = xhatminus(:,i) + (dx1 + 2 * dx2 + 2 * dx3 + dx4) / 6; 
            xhatminus(:,i) = xhatminus(:,i) + sqrt(dt * Q) * [randn; randn; randn] * dt; 
            xhatminus(3,i) = max(0, xhatminus(3,i)); % the ballistic coefficient cannot be negative 
            % auxiliary particle filter 
            [dx1, dx2, dx3, dx4] = RungeKutta(xhatminusAux(:,i)); 
            xhatminusAux(:,i) = xhatminusAux(:,i) + (dx1 + 2 * dx2 + 2 * dx3 + dx4) / 6; 
            xhatminusAux(:,i) = xhatminusAux(:,i) + sqrt(dt * Q) * [randn; randn; randn] * dt; 
            xhatminusAux(3,i) = max(0, xhatminusAux(3,i)); % the ballistic coefficient cannot be negative 
        end 
        zhat = sqrt(M^2 + (xhatminus(1,i)-a)^2); 
        vhat(i) = z - zhat; 
        zhatAux = sqrt(M^2 + (xhatminusAux(1,i)-a)^2); 
        vhatAux(i) = z - zhatAux; 
    end 
    % Note that we need to scale all of the q(i) probabilities in a way 
    % that does not change their relative magnitudes. 
    % Otherwise all of the q(i) elements will be zero because of the 
    % large value of the exponential. 
    % standard particle filter 
    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 
    % auxiliary particle filter 
    vhatscaleAux = max(abs(vhatAux)) / 4; 
    qsumAux = 0; 
    for i = 1 : N 
        qAux(i) = exp(-(vhatAux(i)/vhatscaleAux)^2); 
        qsumAux = qsumAux + qAux(i); 
    end 
    % Regularize the probabilities - this is conceptually identical to the 
    % auxiliary particle filter - increase low probabilities and decrease 
    % high probabilities. 
    % Large k means low regularization (k = infinity is identical to the 
    % standard particle filter). Small k means high regularization (k = 1 
    % means that all probabilities are equal).  
    kAux = 1.1;  
    qAux = ((kAux - 1) * qAux + mean(qAux)) / kAux; 
    % Normalize the likelihood of each a priori estimate. 
    for i = 1 : N 
        qAux(i) = qAux(i) / qsumAux; 
    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 
                xhatplus(:,i) = xhatminus(:,j); 
                xhatplus(3,i) = max(0,xhatplus(3,i)); % the ballistic coefficient cannot be negative 
                break; 
            end 
        end 
    end 
    % The standard particle filter estimate is the mean of the particles. 
    xhat = mean(xhatplus')'; 
    % Resample the auxiliary particle filter 
    for i = 1 : N 
        u = rand; % uniform random number between 0 and 1 
        qtempsum = 0; 
        for j = 1 : N 
            qtempsum = qtempsum + qAux(j); 
            if qtempsum >= u 
                xhatplusAux(:,i) = xhatminusAux(:,j); 
                xhatplusAux(3,i) = max(0,xhatplusAux(3,i)); % the ballistic coefficient cannot be negative 
                break; 
            end 
        end 
    end 
    % The auxiliary particle filter estimate is the mean of the particles. 
    xhatAux = mean(xhatplusAux')'; 
    % Save data for plotting. 
    xArray = [xArray x]; 
    xhatArray = [xhatArray xhat]; 
    xhatAuxArray = [xhatAuxArray xhatAux]; 
end 
 
close all; 
t = 0 : T : tf; 
figure;  
semilogy(t, abs(xArray(1,:) - xhatArray(1,:)), 'b-'); hold; 
semilogy(t, abs(xArray(1,:) - xhatAuxArray(1,:)), 'r:');  
set(gca,'FontSize',12); set(gcf,'Color','White'); 
xlabel('Seconds'); 
ylabel('Altitude Estimation Error'); 
legend('Standard particle filter', 'Auxiliary particle filter'); 
 
figure;  
semilogy(t, abs(xArray(2,:) - xhatArray(2,:)), 'b-'); hold; 
semilogy(t, abs(xArray(2,:) - xhatAuxArray(2,:)), 'r:'); 
set(gca,'FontSize',12); set(gcf,'Color','White'); 
xlabel('Seconds'); 
ylabel('Velocity Estimation Error'); 
legend('Standard particle filter', 'Auxiliary particle filter'); 
 
figure;  
semilogy(t, abs(xArray(3,:) - xhatArray(3,:)), 'b-'); hold; 
semilogy(t, abs(xArray(3,:) - xhatAuxArray(3,:)), 'r:');  
set(gca,'FontSize',12); set(gcf,'Color','White'); 
xlabel('Seconds'); 
ylabel('Ballistic Coefficient Estimation Error'); 
legend('Standard particle filter', 'Auxiliary particle filter'); 
 
figure; 
plot(t, xArray(1,:)); 
set(gca,'FontSize',12); set(gcf,'Color','White'); 
xlabel('Seconds'); 
ylabel('True Position'); 
 
figure; 
plot(t, xArray(2,:)); 
title('Falling Body Simulation', 'FontSize', 12); 
set(gca,'FontSize',12); set(gcf,'Color','White'); 
xlabel('Seconds'); 
ylabel('True Velocity'); 
 
for i = 1 : 3 
    StdRMSErr(i) = sqrt((norm(xArray(i,:) - xhatArray(i,:)))^2 / tf / dt); 
    AuxRMSErr(i) = sqrt((norm(xArray(i,:) - xhatAuxArray(i,:)))^2 / tf / dt); 
end 
disp(['Standard particle filter RMS error = ', num2str(StdRMSErr)]); 
disp(['Auxiliary particle filter RMS error = ', num2str(AuxRMSErr)]); 
 
function [dx1, dx2, dx3, dx4] = RungeKutta(x) 
% Fourth order Runge Kutta integration for the falling body system. 
global rho0 g k dt 
dx1(1,1) = x(2); 
dx1(2,1) = rho0 * exp(-x(1)/k) * x(2)^2 / 2 * x(3) - g; 
dx1(3,1) = 0; 
dx1 = dx1 * dt; 
xtemp = x + dx1 / 2; 
dx2(1,1) = xtemp(2); 
dx2(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g; 
dx2(3,1) = 0; 
dx2 = dx2 * dt; 
xtemp = x + dx2 / 2; 
dx3(1,1) = xtemp(2); 
dx3(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g; 
dx3(3,1) = 0; 
dx3 = dx3 * dt; 
xtemp = x + dx3; 
dx4(1,1) = xtemp(2); 
dx4(2,1) = rho0 * exp(-xtemp(1)/k) * xtemp(2)^2 / 2 * xtemp(3) - g; 
dx4(3,1) = 0; 
dx4 = dx4 * dt; 
return;