In the previous post, we discuss about the implementation of Kalman filter for static state (the true value of the object’s states are constant over time). In addition, the Kalman filter algorithm is applied to estimate single true value.

This time, we’re going to look at how to apply the algorithm on the context of dynamic state (the true value of the object being measured is governed by a certain equation). To be more specific, the algorithm is used to estimate multiple true values of an object being measured.

Code

You can find the implementation of Kalman filter in Python on my Github repo.

Feel free to send a pull request!

How it works?

Suppose we’re going to estimate the true value of position & velocity of a moving object in a single direction (x-axis).

Here are the general steps in applying Kalman filter.

NB: Variables with all capital letters denote matrix (ex: VAR_NAME refers to a matrix called VAR_NAME)

For each measurement (observation) iteration, do the followings.

Calculate the predicted state estimate

Taking the initial estimate of position & velocity as the PREVIOUS_STATE, calculate the predicted state estimate by the following.

PREDICTED_STATE_ESTIMATE = STATE_MULTIPLIER * PREVIOUS_STATE 
                              + CONTROL_VARIABLE_MULTIPLIER * CONTROL_VARIABLE \
                              + STATE_PREDICTION_PROCESS_ERROR

Calculate the predicted state covariance matrix

Taking the initial estimate covariance matrix as the PREVIOUS_STATE_COVARIANCE_MATRIX, calculate the predicted state of the covariance matrix by the following.

PREDICTED_STATE_COVARIANCE_MATRIX = STATE_MULTIPLIER * PREVIOUS_STATE_COVARIANCE_MATRIX * STATE_MULTIPLIER_transposed \
                                    + PREDICTED_STATE_COVARIANCE_MATRIX_PROCESS_ERROR

Calculate the Kalman gain

OBSERVATION_ERRORS_COVARIANCE = [[(OBSERVATION_ERROR_POSITION)^2, 0.0] 
                                 [0.0, (OBSERVATION_ERROR_VELOCITY)^2)]]

TRANSFORMER_H = np.array([[1.0, 0.0], [0.0, 1.0]])

KALMAN_GAIN = (PREDICTED_STATE_COVARIANCE_MATRIX * TRANSFORMER_H_TRANSPOSED) \
               / ((TRANSFORMER_H * PREDICTED_STATE_COVARIANCE_MATRIX)) * TRANSFORMER_H_TRANSPOSED) \
               + OBSERVATION_ERRORS_COVARIANCE)

Calculate observations where non-observation errors are included

TRANSFORMER_C = [[1.0, 0.0]
                 [0.0, 1.0]]

OBSERVATION_WITH_NON_OBS_ERRORS = (TRANSFORMER_C * OBSERVATION) + NEW_OBSERVATION_PROCESS_ERROR

Calculate current state estimate

TRANSFORMER_H = [[1.0, 0.0]
                 [0.0, 1.0]]
                 
OBSERVATION_AND_PREDICTED_STATE_ESTIMATE_DIFF = OBSERVATION_WITH_NON_OBS_ERRORS - (TRANSFORMER_H * PREDICTED_STATE_ESTIMATE)

CURRENT_STATE_ESTIMATE = PREDICTED_STATE_ESTIMATE + (KALMAN_GAIN * OBSERVATION_AND_PREDICTED_STATE_ESTIMATE_DIFF)

Calculate current state estimate covariance matrix

TRANSFORMER_H = [[1.0, 0.0]
                 [0.0, 1.0]]

I = [[1.0, 0.0]
     [0.0, 1.0]]

CURRENT_STATE_ESTIMATE_COVARIANCE_MATRIX = (I - (KALMAN_GAIN * TRANSFORMER_H)) * PREDICTED_STATE_COVARIANCE_MATRIX

Current state estimate & current state estimate covariance matrix becomes the previous state for the next measurement iteration.

The next measurement iteration follows the same steps as above.