08. Tracking

Kalman Filter

Combine noisy measurements with predictions of how the state changes to get better estimate of real state.

Tracking: inference over time.

Can simplify the problem by assuming linear dynamics and Gaussian noise. An unscented Kalman filter can deal with non-linear state transitions, but still assumes Gaussian noise.

Task: at each time point (and in real-time), re-compute the estimate of position.

Recursive estimation: decompose this into:

Minimal example - running average:

At=αat−1+(1−α)yt A_t = \alpha a_{t - 1} + (1 - \alpha) y_t

Where α\alpha is the weight given to the previous estimate and yiy_i is the iith measurement.

This would be sensitive to noise/occlusion

Tracking:

Generalized model:

Issues:

Simplifying assumptions:

1D Kalman Filter

Assumes new state can be obtained by multiplying the old state by a constant did_i and adding noise:

xi∼N(dixi−1,θdi2) x_i \sim N(d_i x_{i - 1}, \theta_{d_i}^2)

In other words:

Xˉi−=di⋅Xˉi−1+(θi−)2=θdi2+(diθi−1+)2 \begin{aligned} \bar{X}^{-}_i &= d_i \cdot \bar{X}^{+}_{i - 1} \\ (\theta^{-}_i)^2 &= \theta_{d_i}^2 + (d_i \theta^{+}_{i - 1})^2 \end{aligned}

TODO what is θdi\theta_{d_i}? Why not just the second term?

TODO what is θ+\theta^{+} and θ−\theta^{-}

Once a measurement arrives, this can be corrected:

xi+=xˉi−θmi2+miyi(θi−)2θmi2+mi2(θi−)2θi+=θmi2(θi−)2θmi2+mi2(θi−)2 \begin{aligned} x_i^+ &= \frac{\bar{x}_i^{-} \theta^2_{m_i} + m_i y_i (\theta^{-}_i)^2} {\theta^2_{m_i} + m^2_i (\theta^{-}_i)^2} \\ \theta_i^{+} &= \sqrt{\frac{\theta^2_{m_i}(\theta^{-}_i)^2} {\theta^2_{m_i} + m^2_i(\theta_i^{-})^2}} \end{aligned}

Note: θ\theta does not depend on yy.

Smoothing: if not running the filte in real time, can run the algorithm forwards and backwards and find the mean between the two predictions.

Kalman in Python

g-h filter:

def g_h_filter(data, x0, dx, g, h, dt=1.):
  """
  Performs g-h filter on 1 state variable with a fixed g and h.

  'data' contains the data to be filtered.
  'x0' is the initial value for our state variable
  'dx' is the initial change rate for our state variable (assumes linear rate of change)
  'g' is the g-h's g scale factor. g * 100% of the estimate comes from the measurement. Should be high for less noisy measurements
  'h' is the g-h's h scale factor - larger h means responds quicker to change, but more vulnerable to noise/outliers
  """
  x_estimate = x0
  results = []
  for x_measurement in data:
    # prediction step
    x_prediction = x_estimate + (dx*dt)
    dx = dx

    # update step
    residual = z - x_prediction # delta between measurement and prediction

    # update rate of change using residual.
    # h determines how quickly the rate of change changes
    dx = dx + h * (residual) / dt

    # Update estimate be weighted average of prediction and measurement
    # g determines weight given to the measurement
    x_estimate = x_prediction + g * residual

Example: system where position is being measured and the object has constant velocity

The distance and velocity can be represented as Gaussian distributions:

dˉ=μd+μvθˉ=θd2+θv2 \begin{aligned} \bar{d} &= \mu_d + \mu_v \\ \bar{\theta} &= \theta_d^2 + \theta_v^2 \end{aligned}

Sum of two Gaussians:

μ=μ1+μ2σ2=σ12+σ22 \begin{aligned} \mu &= \mu_1 + \mu_2 \\ \sigma^2 &= \sigma_1^2 + \sigma_2^2 \end{aligned}

Hence, the prediction can be represented as the sum of the distributions of the previous position and predicted velocity.

Product of two Gaussians:

μ=σ12μ2+σ22μ1σ12+σ22σ2=σ12σ22σ12+σ22 \begin{aligned} \mu &= \frac{\sigma_1^2 \mu_2 + \sigma_2^2 \mu_1}{\sigma_1^2 + \sigma_2^2} \\ \sigma^2 &= \frac{\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2} \end{aligned}

The update step returns the estimated position as the product of the distributions of the new measurement and current estimated position.

Particle Filter

The particle filter allows multiple positions to be predicted, and works with multi-modal and non-Gaussian distributions.

Three probability distributions:

The particle filter processes this into a single probability distribution, the state density (xt∣Zt)(x_t | Z_t).

Comparisons:

Kalman in Python

Algorithm: