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


function [StdErr, EKFErr] = ParticleEx5 
 
% EKF 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 EKF 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 
    xhatplusEKF(:,i) = xhatplus(:,i); % EKF particle filter 
    Pplus(:,:,i) = P; % initial EKF particle filter estimation error covariance 
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; 
xhatEKFArray = 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 filters (time update). 
    xhatminus = xhatplus; 
    xhatminusEKF = xhatplusEKF; 
    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 
            % EKF particle filter 
            [dx1, dx2, dx3, dx4] = RungeKutta(xhatminusEKF(:,i)); 
            xhatminusEKF(:,i) = xhatminusEKF(:,i) + (dx1 + 2 * dx2 + 2 * dx3 + dx4) / 6; 
            xhatminusEKF(:,i) = xhatminusEKF(:,i) + sqrt(dt * Q) * [randn; randn; randn] * dt; 
            xhatminusEKF(3,i) = max(0, xhatminusEKF(3,i)); % the ballistic coefficient cannot be negative 
        end 
        % standard particle filter 
        zhat = sqrt(M^2 + (xhatminus(1,i)-a)^2); 
        vhat(i) = z - zhat; 
        % EKF particle filter 
        zhatEKF = sqrt(M^2 + (xhatminusEKF(1,i)-a)^2); 
        F = [0 1 0; -rho0 * exp(-xhatminusEKF(1,i)/k) * xhatminusEKF(2,i)^2 / 2 / k * xhatminusEKF(3,i) ... 
            rho0 * exp(-xhatminusEKF(1,i)/k) * xhatminusEKF(2,i) * xhatminusEKF(3,i) ... 
            rho0 * exp(-xhatminusEKF(1,i)/k) * xhatminusEKF(2,i)^2 / 2; ... 
            0 0 0]; 
        H = [(xhatminusEKF(1,i) - a) / sqrt(M^2 + (xhatminusEKF(1,i)-a)^2) 0 0]; 
        Pminus(:,:,i) = F * Pplus(:,:,i) * F' + Q; 
        K = Pminus(:,:,i) * H' * inv(H * Pminus(:,:,i) * H' + R); 
        xhatminusEKF(:,i) = xhatminusEKF(:,i) + K * (z - zhatEKF); 
        zhatEKF = sqrt(M^2 + (xhatminusEKF(1,i)-a)^2); 
        vhatEKF(i) = z - zhatEKF; 
    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 
    % EKF particle filter 
    vhatscaleEKF = max(abs(vhatEKF)) / 4; 
    qsumEKF = 0; 
    for i = 1 : N 
        qEKF(i) = exp(-(vhatEKF(i)/vhatscaleEKF)^2); 
        qsumEKF = qsumEKF + qEKF(i); 
    end 
    % Normalize the likelihood of each a priori estimate. 
    for i = 1 : N 
        qEKF(i) = qEKF(i) / qsumEKF; 
    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 EKF particle filter 
    Ptemp = Pplus; 
    for i = 1 : N 
        u = rand; % uniform random number between 0 and 1 
        qtempsum = 0; 
        for j = 1 : N 
            qtempsum = qtempsum + qEKF(j); 
            if qtempsum >= u 
                xhatplusEKF(:,i) = xhatminusEKF(:,j); 
                xhatplusEKF(3,i) = max(0,xhatplusEKF(3,i)); % the ballistic coefficient cannot be negative 
                Pplus(:,:,i) = Ptemp(:,:,j); 
                break; 
            end 
        end 
    end 
    % The EKF particle filter estimate is the mean of the particles. 
    xhatEKF = mean(xhatplusEKF')'; 
    % Save data for plotting. 
    xArray = [xArray x]; 
    xhatArray = [xhatArray xhat]; 
    xhatEKFArray = [xhatEKFArray xhatEKF]; 
end 
 
close all; 
t = 0 : T : tf; 
figure;  
semilogy(t, abs(xArray(1,:) - xhatArray(1,:)), 'b-'); hold; 
semilogy(t, abs(xArray(1,:) - xhatEKFArray(1,:)), 'r:');  
set(gca,'FontSize',12); set(gcf,'Color','White'); 
xlabel('Seconds'); 
ylabel('Altitude Estimation Error'); 
legend('Standard particle filter', 'EKF particle filter'); 
 
figure;  
semilogy(t, abs(xArray(2,:) - xhatArray(2,:)), 'b-'); hold; 
semilogy(t, abs(xArray(2,:) - xhatEKFArray(2,:)), 'r:'); 
set(gca,'FontSize',12); set(gcf,'Color','White'); 
xlabel('Seconds'); 
ylabel('Velocity Estimation Error'); 
legend('Standard particle filter', 'EKF particle filter'); 
 
figure;  
semilogy(t, abs(xArray(3,:) - xhatArray(3,:)), 'b-'); hold; 
semilogy(t, abs(xArray(3,:) - xhatEKFArray(3,:)), 'r:');  
set(gca,'FontSize',12); set(gcf,'Color','White'); 
xlabel('Seconds'); 
ylabel('Ballistic Coefficient Estimation Error'); 
legend('Standard particle filter', 'EKF 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 
    StdErr(i) = sqrt((norm(xArray(i,:) - xhatArray(i,:)))^2 / tf / dt); 
    EKFErr(i) = sqrt((norm(xArray(i,:) - xhatEKFArray(i,:)))^2 / tf / dt); 
end 
disp(['Standard particle filter RMS error = ', num2str(StdErr)]); 
disp(['EKF particle filter RMS error = ', num2str(EKFErr)]); 
 
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;