ParticleEx4.m

[dx1, dx2, dx3, dx4] = RungeKutta(x);
x = x + (dx1 + 2 * dx2 + 2 * dx3 + dx4) / 6;
x = x + sqrt(dt * Q) * [randn; randn; randn] * dt;
% 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)
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
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)
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
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;
% means that all probabilities are equal).
kAux = 1.1;
qAux = ((kAux - 1) * qAux + mean(qAux)) / kAux;
% Normalize the likelihood of each a priori estimate.
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].
if qtempsum >= u
xhatplusAux(:,i) = xhatminusAux(:,j);
xhatplusAux(3,i) = max(0,xhatplusAux(3,i)); % the ballistic coefficient cannot be negative
xhatplus(3,i) = max(0,xhatplus(3,i)); % the ballistic coefficient cannot be negatiarge 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);
% auxiliary particle filter
[dx1, dx2, dx3, dx4] = RungeKutta(xhatminusAux(:,i));
xhatminusAux(:,i) = xhatminusAux(:,i) + (dx1 + 2 * dx2 + 2 * dx3 + dx4) / 6;
for i = 1 : N
xhatplus(:,i) = x + sqrt(P) * [randn; randn; randn]; % standard particle filter
xhatplusAux(:,i) = xhatplus(:,i); % auxiliary particle filter
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
break;
end
end
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).
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
qAux(i) = qAux(i) / qsumAux;
end
% Resample the standard particle filter
for i = 1 : N
u = rand; % uniform random number between 0 and 1
end
% Regularize the probabilities - this is conceptually identical to the
% auxiliary particle filter - increase low probabilities and decrease
for i = 1 : N
u = rand; % uniform random number between 0 and 1
qtempsum = 0;
for j = 1 : N
qtempsum = qtempsum + qAux(j);
qtempsum = 0;
for j = 1 : N
qtempsum = qtempsum + q(j);
if qtempsum >= u
xhatplus(:,i) = xhatminus(:,j);
xhatminus = xhatplus;
xhatminusAux = xhatplusAux;
for i = 1 : N
for tau = dt : dt : T
% Fourth order Runge Kutta ingegration
end
% The standard particle filter estimate is the mean of the particles.
xhat = mean(xhatplus')';
% Resample the auxiliary particle filter
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
% high probabilities.
% Large k means low regularization (k = infinity is identical to the
% standard particle filter). Small k means high regularization (k = 1
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
vhatscaleAux = max(abs(vhatAux)) / 4;