State Estimation with Kalman Filter

Kalman Filter (aka linear quadratic estimation (LQE)) is an algorithm which can be used to estimate precise states of a moving object by feeding a series of noisy sensor inputs over time.

The Kalman Filter and its derivatives namely, “Extended Kalman Filter (EKF)” and “Unscented Kalman Filter” are highly reputed in the field of information processing. The most famous early use of the Kalman filter was in the Apollo navigation computer that took Neil Armstrong to the moon, and (most importantly) brought him back. Today, Kalman filters are at work in every satellite navigation device, every smart phone, and many computer games.

The Kalman filter requires a dynamic model of the system to predict next states of the system. The dynamic model can be motion laws or other equations that has ability to calculate the next state with use of correct coordinates.

Kalman Filter performs on two main operations. They are,

• Estimating the next state of the machine
• Correcting the estimated state with actual measurements

The following picture shows estimated location (after the correction) against measured location and the true location.

Modeling Kalman Filter

To model the scenario with Kalman filter, it requires to model two equations.

1. Process model (State Equation)

• xt is the state vector containing the terms of interest for the system (e.g., position, velocity, heading) at time t
• ut is the vector containing any control inputs (steering angle, throttle setting, braking force)
• Ft is the state transition matrix which applies the effect of each system state parameter at time t-1 on the system state at time t (e.g., the position and velocity at time t-1 both affect the position at time t)
• Bt is the control input matrix which applies the effect of each control input parameter in the vector ut on the state vector (e.g., applies the effect of the throttle setting on the system velocity and position)
• w(t) is the vector containing the process noise terms (unknown) for each parameter in the state vector. The process noise is assumed to be drawn from a zero mean multivariate normal distribution with covariance given by the covariance matrix Q(t ).

1. Measurement model (measurement equation)

• zt is the vector of measurements
• Ht is the transformation matrix that maps the state vector parameters into the measurement domain
• vt is the vector containing the measurement noise terms (known) for each observation in the measurement vector. Like the process noise, the measurement noise is assumed to be zero mean Gaussian white noise with covariance Rt.

For a simpler example let’s consider a scenario of a car, moving on X- axis with a constant acceleration (a). Suppose car emits its X coordinate periodically. But due to the measurement noises, the measurement can be vary from the actual location.

State vector contains the location and the velocity of the car over the X axis.

Using motion equations,

Now this is the state equation in matrix form using 1 and 2,

Suppose we are reading speed and the location and the speed with white measurement noises. This is the measurement model in matrix form.

With 3 and 4, we can estimate the states of the machine. If you are using a Kalman implementation library, it will do the rest of the calculation given 3 and 4 models. i.e Apache Commons Math Kalman Filter implementation.

Kalman Filter equations

Kalman Filter maintains the estimates of the state and the error covariance matrix of the state estimation.

Notations:

• X(t|t) — Estimate of x(t) given measurements z(t) and z (t-1),….
• X(t+1|t) — Estimate of x(t+1) given measurements z(t) and z (t-1),…
• P(t|t) — Covariance of X(t) given z(t), z(t-1),…
• P(t+1|t) — Covariance of X(t+1) given z(t), z(t-1),…

State Estimation

Known are x(t|t), u(t ), P(t|t) and the new measurement z(t+1).

Time Update

1. State Prediction x(t +1|t )= F(t ) x(t|t) + G(t ) u(t )
2. Measurement Prediction: z(t+1|t)= H(t)x(t+1|t )

Measurement Update

1. Measurement Residual: w(t+1)= z(t+1) — z(t+1|t)
2. Updated State Estimate: x(t+1|t+1)= x(t+1|t) +W(t+1)w(t+1)

Where W(t+1) is called Kalman Gain in state covariance estimation.

State covariance Estimation

1. State prediction covariance: P(t +1|t ) = F(t)P(t|t )F(t )’+Q(t )
2. Measurement prediction covariance: S(t +1) = H(t +1)P(t +1|t)H(t +1)’+R(t +1)
3. Filter Gain: W(t +1) = P(t +1|t )H(t +1)’ S(t +1)-1
4. Updated state covariance: P(t +1|t +1) = P(t +1|t ) — W(t +1)S(t +1)W(t +1)’

Hopefully, I’ll cover the implementation of a Kalman filter using Apache Math library in my next post!

Cheers!