Kalman and Particle Filter

1. Kalman and Particle Filter
Presented by
Wasiq Ali
Advisor
Professor Yaan Li
Presented to
Acoustics Research Group
School of Marine Science and Technology
Northwestern Polytechnical University
June 29, 2018

2. Contents
• Kalman Filter
– Introduction
– Mathematical modeling
– Work flow
– Example
– Simulation
– Result
– Applications

3. Introduction
• Kalman filter is an optimal (linear) estimator or
optimal recursive data processing algorithm.
• It is an optimal solution for many tracking and data
prediction tasks, constructed as mean squared error
minimizer, relates to maximum likelihood statistics.
• Kalman filter is “best” estimate based on all
previous measurements.
• The resulting estimator is statistically optimal with
respect to any quadratic function of estimation error.

4. Introduction (cont...)
• It is only a tool.
• It does not solve any problem all by itself, although it
can make it easier for you to do it.
• Its not a physical tool, but a mathematical one.
• Practically, it is certainly one of the greatest
discoveries in the history of statistical estimation
theory and possibly the greatest discovery in 20th
century.
• Kalman filter provides a means for interfering the
missing information from indirect (noisy)
measurements.

5. Theory of Kalman filter
Kalman also presented a prescription of the optimal MSE
filter. However Kalman's prescription has some advantages
over Weiner's; it sidesteps the need to determine the impulse
response of the filter, something which is poorly suited to
numerical computation.
Kalman described his filter using state space techniques,
which unlike Wiener's prescription, enables the filter to be
used as either a smoother, a filter or a predictor. The latter of
these three, the ability of the Kalman filter to be used to
predict data has proven to be a very useful function.
It has lead to the Kalman filter being applied to a wide range
of tracking and navigation problems.

6. State Space Model
Defining the filter in terms of state space methods also
simplifies the implementation of the filter in the discrete
domain, another reason for its widespread appeal.
For better explanation of Kalman filter derivation by
using state space model we shift to another pdf file.

7. Carnegie Mellon University
December 8, 2000
Work flow of Kalman filter

8. Carnegie Mellon University
December 8, 2000
Example MATLAB Code
% x(k+1) = Fk * x(k) + Wk; prediction model
% y(k) = Hk * x(k) + Vk; Observation model
N = 365;
Fk = [1]; % State transition matrix
X = zeros(N,1); % Initialization state variable
W = 12*randn(N,1); % Structural process noise
X(1) = 100; % Initial state
for k = 2:N % state equation
X(k) = Fk * X(k-1) + W(k-1);
end
Hk = [2]; % Observation matrix
Y = zeros(N,1); % Initialization of observation variables
V = 20*randn(N,1); % Structural observation noise
for k = 1:N % Observation equation
Y(k) = Hk * X(k) + V(k);
end

9. Carnegie Mellon University
December 8, 2000
Q = cov(W); % The covariance of process noise;
R = cov(V); % The covariance of observation noise;
Xupdate = zeros(N,1);
Xupdate(1) = Y(1); % Initialize the first which one observed.
Pupdate(1) = 0;
for k = 2:N % Five core equations
Xpredict(k) = Fk * Xupdate(k-1);
Ppredict(k) = Fk * Pupdate(k-1) * Fk' + Q;
K = Ppredict(k) * Hk * (Hk * Ppredict(k) * Hk' + R).^-1;
Xupdate(k) = (1 - K * Hk) * Xpredict(k) + K * Y(k);
Pupdate(k) = (1 - K * Hk) * Ppredict(k);
End
plot(1:N,X,'B','linewidth',2);hold on;
plot(1:N,Y,'K','linewidth',2);hold on;
plot(1:N,Xupdate,'R','linewidth',2);hold on;
plot(1:N,abs(Xupdate-X),'m','linewidth',2);hold off;
legend('True value','Observed value','Filter Value','Error');

10. Carnegie Mellon University
December 8, 2000
Result

11. Carnegie Mellon University
December 8, 2000
Applications of Kalman Filter
• A tool almost exclusively use for
two purposes:
 Estimation
 Performance analysis of estimators
• Radar tracking of an airplane.
• GPS tracking of Vehicle.
• Satellite communication tracking.
• Target Detection in defense
applications.
• Image Processing.
• Flow of rivers during floods.
• Trajectories of celestial bodies.
• Control of complex dynamic
systems such as continuous
manufacturing processes.

12. Contents
• Particle Filter
– Introduction
– Mathematical modeling
– Work flow
– Example
– Simulation
– Result
– Applications
– Demo

13. Introduction
• The particle filter (PF) was introduced in 1993 as a
numerical approximation to the nonlinear Bayesian
filtering problem.
• Bayesian state estimator that uses discrete particles to
approximate the posterior distribution of estimated
state.
• Great way to track the state of a dynamic system.
• Recursive model for nonlinear systems.
• Process and measurement noise follow arbitrary non
gaussian distribution.

14. Introduction (cont..)
• Particle filters, which let you use the full, complex
model, but just find an approximate solution instead.
• Particle filtering uses a genetic mutation-selection
sampling approach, with a set of particles (also called
samples) to represent the posterior distribution of
some stochastic process given noisy and/or partial
observations.
• The main reason is that for a lot of large or high-
dimensional problems, particle filters are tractable
whereas Kalman filters are not.

15. Theory and Mathematical Modeling
• Bayes Filtering
• used to discuss the method
of using a predict/update
cycle to estimate the state
of a dynamical system from
sensor measurements
p(xt | d0…t)
p(zt | xt),
p(xt | xt-1, ut-1)
X is the state variable
Xt is the state variable at time t
U is the inputs to your system
z is the observations made by
the sensors
d just refers to inputs and
observations together
probability of xt given
all the data we’ve seen
so far
perceptual model,
action model

16. Theory and Mathematical Modeling
What we are given is the inputs, the
observations, the perceptual model, which is
the probability that you’ll see a particular
observation given that you’re in some state at
time t, and the action model, which is the
probability that you’ll end up in state xt at time
t, assuming that you started in state xt-1 at time
t-1, and input ut-1 to your system.

17. The basic premise of particle filters
• The basic idea of particle filters is that any pdf can be
represented as a set of samples (particles).
• The density of your samples in one area of the state
space represents the probability of that region.
• This method can represent any arbitrary distribution,
making it good for non-Gaussian, multi-modal pdfs.
• The key idea is that you find an approximate
representation of a complex model (any arbitrary pdf)
rather than an exact representation of a simplified
mode (Gaussians).

18. Carnegie Mellon University
December 8, 2000
Parameters of Particle filter
• Number of particles
• Initial particle location
• State estimation function
• Measurement Likelihood Function
• Resampling Policy
• State Estimation Method

19. Carnegie Mellon University
December 8, 2000
Number of Particles
To specify the number of particles, use the initialize method. Each
particle is a hypothesis of the current state. The particles are
distributed across your state space based on either a specified
mean and covariance, or on the specified state bounds. Depending
on the State Estimation Method property, either the particle with
the highest weight or the mean of all particles is taken to
determine the best state estimate. The default number of particles
is 1000. Unless performance is an issue, do not use fewer than
1000 particles. A higher number of particles can improve the
estimate but sacrifices performance speed, because the algorithm
has to process more particles. Tuning the number of particles is
the best way to affect your particle filters performance.

20. Carnegie Mellon University
December 8, 2000
Initial Particle Location
When you initialize your particle filter, you can specify the initial
location of the particles using Mean and covariance and State bounds
Your initial state is defined as a mean with a covariance relative to
your system. This mean and covariance correlate to the initial
location and uncertainty of your system. The Particle Filter object
distributes particles based on your covariance around the given mean.
The algorithm uses this distribution of particles to get the best
estimation of state, so an accurate initialization of particles helps to
converge to the best state estimation quickly.
If an initial state is unknown, you can evenly distribute your particles
across a given state bounds. The state bounds are the limits of your
state.

21. Carnegie Mellon University
December 8, 2000
State Transition Function
The state transition function, State Transition Fcn, of a particle
filter helps to evolve the particles to the next state. It is used
during the prediction. In the Particle Filter object, the state
transition function is specified as a callback function that takes
the previous particles, and any other necessary parameters, and
outputs the predicted location. By default, the state transition
function assumes a Gaussian motion model with constant
velocities. The function uses a Gaussian distribution to
determine the position of the particles in the next time step. You
also must specify system noise in State Transition Fcn. Without
random noise applied to the predicted system, the particle filter
does not function as intended.

22. Carnegie Mellon University
December 8, 2000
Measurement Likelihood Function
After predicting the next state, you can use measurements from
sensors to correct your predicted state. By specifying a Measurement
Likelihood Fcn in the Particle Filter object, you can correct your
predicted particles using the correct function. This measurement
likelihood function, by definition, gives a weight for the state
hypotheses (your particles) based on a given measurement. Essentially,
it gives you the likelihood that the observed measurement actually
matches what each particle observes. This likelihood is used as a
weight on the predicted particles to help with correcting them and
getting the best estimation. Although the prediction step can prove
accurate for a small number of intermediate steps, to get accurate
tracking, use sensor observations to correct the particles frequently.

23. Carnegie Mellon University
December 8, 2000
Resampling Policy
The resampling of particles is a vital step for continuous tracking of objects.
It enables you to select particles based on the current state, instead of using
the particle distribution given at initialization. By continuously resampling
the particles around the current estimate, you can get more accurate tracking
and improve long-term performance. When you call correct, the particles
used for state estimation can be resampled depending on the Resampling
Policy property specified in the Particle Filter object. This property is
specified as a Resampling Policy object. The minimum effective particle
ratio is a measure of how well the current set of particles approximates the
posterior distribution. The number of effective particles is calculated by:

24. Carnegie Mellon University
December 8, 2000
State Estimation Method
The final step of the particle filter workflow is the selection
of a single state estimate. The particles and their weights
sampled across the distribution are used to give the best
estimation of the actual state. However, you can use the
particles information to get a single state estimate in multiple
ways. With the Particle Filter object, you can either choose
the best estimate based on the particle with the highest
weight or take a mean of all the particles. Specify the
estimation method in the State Estimation Method property
as either 'mean'(default) or 'maxweight'.

25. Steps of Algorithm
• How do you sample from your posterior?
• You had some belief from the last time step that you
know how to update with your motion model. (prior
belief q(x) )
• Sample from q(x), and then for each sample that you
made, update it using what we will call an ‘importance
weight’, based on the observations made.
• To start the algorithm, we need the initial belief state,
p(x0). This is just our initial guess of the pdf. For robot
localization, if we have no idea, we can just scatter
particles all over the map.
• Then loop with three phases: prediction, update, and
resample.

26. The formalization of the steps
calculating this equation from right to left

27. Carnegie Mellon University
December 8, 2000
The formalization of the steps
• In the prediction step, we take each particle
and add a random sample from the motion
model.
• The update step , weight that is equal to the
probability of observing the sensor
measurements from that particle’s state.
• In the resample step, a new set of particles is
chosen so that each particle survives in
proportion to its weight.

28. Carnegie Mellon University
December 8, 2000
Work flow of Particle Filter

29. Carnegie Mellon University
December 8, 2000
Example MATLAB Code
x = 0.1; % initial actual state
x_N = 1; % Noise covariance in the system
x_R = 1; % Noise covariance in the measurement
T = 75; % duration the chase (i.e. number of iterations).
N = 100; % The number of particles the system generates.
V = 2; %define the variance of the initial estimate
x_P = []; % define the vector of particles
for i = 1:N
x_P(i) = x + sqrt(V) * randn; % make randomly particles
End
z_out = [x^2 / 20 + sqrt(x_R) * randn]; %the actual output
vector for measurement values (bird position).
x_out = [x]; %the actual output vector for measurement values.
x_est = [x]; % time by time output of the particle filters
estimate
x_est_out = [x_est]; % the vector of particle filter estimates.

30. Carnegie Mellon University
December 8, 2000
for t = 1:T
x = 0.5*x + 25*x/(1 + x^2) + 8*cos(1.2*(t-1)) + sqrt(x_N)*randn;
z = x^2/20 + sqrt(x_R)*randn; % update flight and bird position
for i = 1:N
x_P_update(i) = 0.5*x_P(i) + 25*x_P(i)/(1 + x_P(i)^2) +
8*cos(1.2*(t-1)) + sqrt(x_N)*randn; %update the particles
z_update(i) = x_P_update(i)^2/20;
P_w(i) = (1/sqrt(2*pi*x_R)) * exp(-(z - z_update(i))^2/(2*x_R));
end
P_w = P_w./sum(P_w); %normalize to form probability distribution
for i = 1 : N
x_P(i)= x_P_update(find(rand <= cumsum(P_w),1));%To find new value
end
x_est = mean(x_P); % Save data in arrays for later plotting
x_out = [x_out x];
z_out = [z_out z];
x_est_out = [x_est_out x_est];
end
t = 0:T;
figure(1);
clf
plot(t, x_out, '.-b', t, x_est_out, '-.r','linewidth',3);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time step'); ylabel('Quail flight position');
legend('True flight position', 'Particle filter estimate');

31. Carnegie Mellon University
December 8, 2000
Result

32. Carnegie Mellon University
December 8, 2000
Applications of Particle Filter
• Particle filtering is becoming an important method
for solving difficult problems in communications.
• It is useful for online state estimation when
measurements and a system model, that relates
model states to the measurements, are available.
• Particle filter is widely used for tracking of robot
like cars.
• A big use in computer vision and image processing
field.

33. Carnegie Mellon University
December 8, 2000
Demo of Particle Filter