State Estimation

Kalman Filter

Background

Although this is simple to understand, it is important as a foundation to understand variations of the KF.

Primarily used in systems that follow a linear relationship i.e. monotonically increasing/decreasing.

The KF is an optimal filter i.e. it combines the prior state estimate with the measurement in a way that minimizes the uncertainty of the current state estimate.

Usually, measurement errors are distributed normally. Therefore, the Kalman Filter (KF) design assumes a Gaussian distribution of the measurement errors.

Goal is to ensure Kalman Filter convergence:

• Kalman Gain should gradually decrease until it reaches a steady state i.e. converges.
• When Kalman Gain is low, the weight of the noisy measurements is also low.
• A meaningful initialization value will achieve a faster convergence.

Note

At large values of $n$, having to recalculate the estimate would be computationally expensive, therefore, by keeping $k_n$ in memory, we can use it to calculate $k_{n+1}$.

The Algorithm

Step 0: Initialization

• Following Initialization (which only occurs once), is Prediction.
• Outputs:
  • Initial System State $({\hat{x}}_{0,0})$
  • Initial State Variance $(p_{0,0})$

Step 1: Measurement

• Outputs:
  • Measured System State $(z_n)$
  • Measured Variance $(r_n)$

Step 2: State Update

• Inputs:
  • Measured Value $(z_n)$
  • A Measurement Variance $(r_n)$
  • Prior Predicted System State Estimate $({\hat{x}}_{n,n-1})$
  • Prior Predicted System State Estimate Variance $(p_{n,n-1})$
• Outputs after Calculating $K_n$:
  • Current System State Estimate $({\hat{x}}_{n,n})$
  • Current State Estimate Variance $(p_{n,n})$

Step 3: Prediction

• Extrapolates the current system state estimate and its variances to the next system state based on the dynamic model of the system.

The Five Kalman Filter Equations

Update

1. State Update ${\hat{x}}_{n,n}={\hat{x}}_{n,n-1}+K_n(z_n-{\hat{x}}_{n,n-1})$
2. Covariance Update $p_{n,n}=(1-K_n)p_{n,n-1}$
3. Kalman Gain $K_n=\frac{p_{n,n-1}}{p_{n,n-1}+r_n}$

Predict

4. State Extrapolation i.e. Dynamic Model/State Space Model
5. Covariance Extrapolation i..e. Predictor Covariance Equation

The Kalman Gain

The Kalman Gain $(K_n)$, is a factor/weight that provides information on whether to trust the measurement or prediction more.

• High $K_n$ - More trust in measurement step, less trust in prediction step.
• Low $K_n$ - More trust in prediction step, less trust in measurement step.

See Deriving the Kalman Gain

Process Noise

Process Noise, $w_n$, is described as the uncertainty in the system dynamic model in the real world as there will always be some sort of error in measurements.

Process Noise Variance is denoted by the letter $q$.

References

KFGU Section 1