1 of 33

�CS60055: Ubiquitous Computing

Statistical Filters

INDIAN INSTITUTE OF TECHNOLOGY

KHARAGPUR

Sandip Chakraborty

sandipc@cse.iitkgp.ac.in

Department of Computer Science and Engineering

2 of 33

Statistical Filters – A Primer

Use a series of observed measurements over time to predict unknown variables, while considering statistical noises and other measurement inaccuracies
Kalman Filter

a.k.a Linear quadratic estimation (estimates state of a linear dynamic system)
The filter uses a system's dynamic model (such as, the laws of motion), known control inputs to that system, and multiple sequential measurements (such as from sensors) to form an estimate of the system's varying quantities (its state)

Expected to be better than the estimate obtained by using only a single measurement.

Estimate a joint probability distribution over the variables for each time frame
Constructed as a mean squared error minimizer
A recursive filter (uses the output to its input)
Various variations exist: Simple Kalman Filter, Kalman-Bucy Filter, Information filter, …

Indian Institute of Technology Kharagpur

3 of 33

Kalman Filtering

Assumes that the true state at time k evolves from the state (k-1)

F_k is the state transition model applied to the previous state x_k-1
B_k is the control-input model applied to the control vector u_k
w_k is the process noise, assumed to be drawn from a zero-mean multivariate normal distribution with covariance Q_k (which also needs an estimate from the observations)

At time k, an observation of the true state is estimated as,

H_k is the observation model, maps the true state space to the observed state space
v_k is the observation noise, assumed to be zero-mean Gaussian white noise

Indian Institute of Technology Kharagpur

4 of 33

Kalman Filtering

Inputs:

Measurements: mx1 vector
Measurement Covariance matrix: mxm matrix

System Model:

State transition matrix: nxn matrix
State-to-measurement matrix: mxn matrix
Process noise covariance matrix: nxn matrix

Internal Parameter:

Kalman Gain: nxm matrix (the weight given to the measurements and current-state estimate)

Output:

State variable: nx1 vector
State Covariance matrix: nxn matrix

Indian Institute of Technology Kharagpur

5 of 33

Kalman Filtering

Initialize system state estimate and system state error covariance

Reinitialize system state estimate and system state error covariance

Predict system state and system state error covariance till the measurement time

Compute the Kalman Gain

Estimate system state and system state error covariance till the measurement time

Measurement 1

Measurement 2

Measurement 3+

Indian Institute of Technology Kharagpur

6 of 33

Kalman Filter: Example

Position (p)

PDF

Estimate at k-1

Indian Institute of Technology Kharagpur

7 of 33

Kalman Filter: Example

Position (p)

PDF

Estimate at k-1

Apply motion model (say, odometry) for predicting the position at k

Prediction

Indian Institute of Technology Kharagpur

8 of 33

Kalman Filter: Example

Position (p)

PDF

Estimate at k-1

Apply motion model (say, odometry) for predicting the position at k

Prediction

Measure the observed location of the car at time k (say, by GPS)

Measurement

Indian Institute of Technology Kharagpur

9 of 33

Kalman Filter: Example

Position (p)

PDF

Estimate at k-1

Apply motion model (say, odometry) for predicting the position at k

Prediction

Measure the observed location of the car at time k (say, by GPS)

Measurement

Fusion

Estimate the position at k which is noise-free

Indian Institute of Technology Kharagpur

10 of 33

Extended Kalman Filters (EKF)

The simple Kalman filters use a linearity assumption in the state transition model F_k and the observation model H_k

Good for many applications
But not all applications can be modeled using liner motion

Can we approximate a non-linear motion�to a linear motion?

Indian Institute of Technology Kharagpur

11 of 33

Extended Kalman Filters (EKFs)

EKFs overcome the linearity assumptions

Considers that the next state probability and the measurement probabilities are giverned by non-linear functions g and h as,��x_k = g(u_k, x_k-1) + ε_k��z_k = h(x_k) + δ_k ��where g and h are any arbitray function, and ε and δ are the process noise and observation noise

However, with arbitrary functions, we cannot assume that the noises are drawn from a Gaussian distribution

We need a linear approximation of the functions

Indian Institute of Technology Kharagpur

12 of 33

Extended Kalman Filters (EKFs)

EKFs overcome the linearity assumptions

Considers that the next state probability and the measurement probabilities are giverned by non-linear functions g and h as,��x_k = g(u_k, x_k-1) + ε_k��z_k = h(x_k) + δ_k ��where g and h are any arbitray function, and ε and δ are the process noise and observation noise

However, with arbitrary functions, we cannot assume that the noises are drawn from a Gaussian distribution

We need a linear approximation of the functions

How can we approcimate g and h to a linear function?

Indian Institute of Technology Kharagpur

13 of 33

Extended Kalman Filters (EKF)

Apply Taylor expansion to get a linear approximation of the function at any measurement point

Use the partial derivative to get the slope:�g'(u_k, x_k-1) := ∂g(u_k, x_k-1)/∂x_k-1

We can approximate g using the Taylor expansion�as follows:�g(u_k, x_k-1) ≈ g(u_k, μ_k-1) + g'(u_k, μ_k-1)(x_k-1 - μ_k-1)��where g'(u_k, μ_k-1) is g'(u_k, x_k-1) evaluated at x_k-1 = μ_k-1

Indian Institute of Technology Kharagpur

14 of 33

Extended Kalman Filters (EKF)

We can approximate g using the Taylor expansion�as follows:�g(u_k, x_k-1) ≈ g(u_k, μ_k-1) + g'(u_k, μ_k-1)(x_k-1 - μ_k-1)��where g'(u_k, μ_k-1) is g'(u_k, x_k-1) evaluated at x_k-1 = μ_k-1
u_k is a constant here (based on the measurement�-- the point at which you compute the slope), �μ_k-1 is also a constant. So, the first term returns a�constant value

The approximation returns a linear function of x_k-1

Indian Institute of Technology Kharagpur

15 of 33

Extended Kalman Filters (EKF)

We define G_k = g'(u_k, μ_k-1) (the Jacobian matrix of size n x n)
Then the approximation is represented as follows:� � g(u_k, x_k-1) ≈ g(u_k, μ_k-1) + G_k(x_k-1 - μ_k-1)

So, we can approximate the next state probability as follows:� � p(x_k|u_k, x_k-1) := N(g(u_k, μ_k-1) + G_k(x_k-1 - μ_k-1), Q_k)

�

Indian Institute of Technology Kharagpur

16 of 33

Extended Kalman Filters (EKF)

Similarly, we approximate�� h(x_k) ≈ h(μ'_k) + h'(μ'_k)(x_k - μ'_k)� h(x_k) ≈ h(μ'_k) + H_k(x_k - μ'_k)��where μ'_k is the state that is most likely at the time of linearizing h

Finally, the measurement probability is approximated as:� � z_k := N(h(μ'_k) + H_k(x_k - μ'_k), R_k)

Indian Institute of Technology Kharagpur

17 of 33

Example: Tracking Vehicle Movements

Source: https://junshengfu.github.io/tracking-with-Extended-Kalman-Filter/

Indian Institute of Technology Kharagpur

18 of 33

Example: Tracking Vehicle Movements

Source: https://junshengfu.github.io/tracking-with-Extended-Kalman-Filter/

Indian Institute of Technology Kharagpur

19 of 33

Example: Tracking Vehicle Movements

Source: https://junshengfu.github.io/tracking-with-Extended-Kalman-Filter/

Indian Institute of Technology Kharagpur

20 of 33

Particle Filters

Non-parametric filters: The representation of the state space is represented as a set of samples drawn from the underlying distribution

Remember that Kalman Filter uses a parametric form – use a function based on Gaussian distribution to model the state space
However, a function may not represent all possible samples in a state space, particularly when the state space is not based on some known form, like Gaussian

Indian Institute of Technology Kharagpur

21 of 33

Particle Filters

Non-parametric filter: The representation of the state space is represented as a set of samples drawn from the underlying distribution

Remember that Kalman Filter uses a parametric form – use a function based on Gaussian distribution to model the state space
However, a function may not represent all possible samples in a state space, particularly when the state space is not based on some known form, like Gaussian

Indian Institute of Technology Kharagpur

22 of 33

Particle Filters

Non-parametric filter: The representation of the state space is represented as a set of samples drawn from the underlying distribution

Remember that Kalman Filter uses a parametric form – use a function based on Gaussian distribution to model the state space
However, a function may not represent all possible samples in a state space, particularly when the state space is not based on some known form, like Gaussian

Rather than maintaining a set of parameters for the distribution (i.e., mean, covariance), the particle filter stores a set of samples that represent the distribution!

Indian Institute of Technology Kharagpur

23 of 33

Particle Filters

The samples drawn from the distribution are called the particles:�� X_k = x_k^[1], x_k^[2], …, x_k^[M]^��Each particle represents a hypothesis (a guess) that what the true world state might be at time k given the observations z and the actions u, �� x_k^[m] ~ p( x_k| z_1:k, u_1:k)

As M → ∞, the denser a sub-region of the state space is populated by the samples drawn, the more likely that the true state falls within this expected sub-region

Indian Institute of Technology Kharagpur

24 of 33

Particle Filter Algorithm

Input: χ_k-1, u_k, z_k
Initialize χ'_k = χ_k = ∅
For m=1 to M, do

Sample x_k^[m] ~ p(x_k | u_k, x_k-1^[m])
ω_k^[m] = p(z_k|x_k^[m])
χ'_k = χ'_k + <x_k^[m], ω_k^[m]>

For m=1 to M, do

draw i with probability proportional to ω_k^[i]
Add x_k^[i] to χ_k

Return χ_k

Draw the initial set of particles based on the belief distribution; however, based on the observations, some particles might have a very low probability

Indian Institute of Technology Kharagpur

25 of 33

Particle Filter Algorithm

Input: χ_k-1, u_k, z_k
Initialize χ'_k = χ_k = ∅
For m=1 to M, do

Sample x_k^[m] ~ p(x_k | u_k, x_k-1^[m])
ω_k^[m] = p(z_k|x_k^[m])
χ'_k = χ'_k + <x_k^[m], ω_k^[m]>

For m=1 to M, do

draw i with probability proportional to ω_k^[i]
Add x_k^[i] to χ_k

Return χ_k

Resample the particles based on the probability: The particles having higher probability gets more number of instances, whereas the particles with very low probability vanishes

Importance Resampling

Indian Institute of Technology Kharagpur

26 of 33

Particle Filter Algorithm

Input: χ_k-1, u_k, z_k
Initialize χ'_k = χ_k = ∅
For m=1 to M, do

Sample x_k^[m] ~ p(x_k | u_k, x_k-1^[m])
ω_k^[m] = p(z_k|x_k^[m])
χ'_k = χ'_k + <x_k^[m], ω_k^[m]>

For m=1 to M, do

draw i with probability proportional to ω_k^[i]
Add x_k^[i] to χ_k

Return χ_k

Particles which are more likely to represent the true state gets duplicated whereas the particles that has very low probabilities get vanished from the system!

Indian Institute of Technology Kharagpur

27 of 33

Particle Filter - Visualization

Source: https://towardsdatascience.com/particle-filter-a-hero-in-the-world-of-non-linearity-and-non-gaussian-6d8947f4a3dc

Indian Institute of Technology Kharagpur

28 of 33

Particle Filters: An Example

Indian Institute of Technology Kharagpur

29 of 33

Particle Filters: An Example

Indian Institute of Technology Kharagpur

30 of 33

Particle Filters: An Example

Resampling

Indian Institute of Technology Kharagpur

31 of 33

Particle Filters: An Example

Resampling

Indian Institute of Technology Kharagpur

32 of 33

Design Challenges of Particle Filters

Number of particles

Theoretically, the filter approximates true distribution when the number of particles is close to infinity
In practice, use a large value of M

High variance

Resampling introduces randomness -> the estimator results in a high variance
If we resample too frequently, we lose variety in the sample
Reduce the rate of resampling, or use a low-variance re-sampler

Indian Institute of Technology Kharagpur

33 of 33

Design Challenges of Particle Filters

Divergence between proposed and target distribution

What if the motion model is noisy but the sensors are correct?
Inherent assumption: The sensors are noisy, and the models are correct!

Particle deprivation

Particles are sparse in high dimensional space
Particles near the true state may vanish due to random resampling
Periodically add a small number of new particles uniformly over the space

Indian Institute of Technology Kharagpur