Kalman filter

Theoretical background

LTI system

The implementation of the code is based on the tutorial from this site.

The Kalman filter starts from the state-space representation of a linear time invariant (LTI) system:

The corresponding discrete form is written as:

The transformation of the continuous and discrete state-space representations can be given by:

The measurement of the system is given by:

In the concept of Kalman filter, additional uncertainty is introduced to the state-space representation and observation of the system:

Kalman filter

The Kalman filter is acutally a state observer of the LTI-system.

The Kalman filter operates in a “predict-correct” loop. In the “predict” step, the estimation of the state vector of the next timestep is calculated along with its unceratinty. And in the “correct” (or “update”) step, the measurement of the output vector is obtained and it is used to update the predicted state vector and its unceratinty.

After given an inital estimation of the state vector and its unceratinty, the Kalman filter is able to make its first prediction. Then, after , the measurement of the system output is obtained, the Kalman filter can correct its last prediction by the measured data, get the updated state variables, and make new prediction for the next timestep. The following table provides an intuitive explaination of how kalman filter works:

index	timestep	operation
<c>	<c>	<c>
0	0	initial estimation of and
1	0	predict: make predictions of and
2	1	get the measured system output and its unceratinty
3	1	update: update current state and unceratinty
4	1	predict: make predictions of and
5	2	get the measured system output and its unceratinty
6	2	update: update current state and unceratinty
7	2	predict: make predictions of and
…	…	…
3n-1	n	get the measured system output and its unceratinty
3n	n	update: update current state and unceratinty
3n+1	n	predict: make predictions of and
…	…	…

The corresponding equations for the kalman filter are listed below.

update:

predict:

Note that the last equation in the prediction procedure can also be written as:

Obviously, it is possible to update the state vector several times before it makes its prediction if the Kalman filter receives several measurements at one timestep.

Usage

The Kalman filter is implemented in kalman.py. This file contains a class Kalman, which constructs a kalman filter for a system with state-space representation.

Initialization

The state transition matrix F, input transition matrix G, and obervation matrix H can be defined when creating the Kalman instance. These three matrixes can also be defined or modified after the class instance is created. That is:

kalman_filter = Kalman(F, G, H)

kalman_filter = Kalman()
kalman_filter.F = F
kalman_filter.G = G
kalman_filter.H = H

Note that it is not necessary to set G if the system does not have input.

The state vector and its uncertainty matrix can be accessed by attribute x and P. The initial values of these two variables should be manually defined after instantiation:

kalman_filter.x = x
kalman_filter.P = P

Predict and update

After setting the system matrixes and initial values, the filter is able to predict or update the state vector of the system.

To make predictions, the member function predict can be called with control input u and its covariance matrix Q:

def predict(self,
            u: npt.ArrayLike | None = None,
            Q: npt.ArrayLike | None = None
            ) -> Kalman: ...

If the system has no control input, u can be set to None. If Q is not given, the lastest setted value for Q will be used.

To update the state vector and its uncertainty, the member function update can be called with measured output z and its uncertainty matrix R:

def update(self,
           z: npt.NDArray,
           R: npt.NDArray | None = None
           ) -> Kalman: ...

If R is not given, its latest setted value will be used.

Filter property

The following attributes of the kalman filter can be obtained by direct access to its property name:

name	description	comment
`F`	state transition matrix	can be set at any time
`G`	input transition matrix	can be set at any time
`H`	observation matrix	can be set at any time
`x`	state vector	can only be set once
`P`	state vector covariance	can only be set once
`K`	Kalman gain matrix	read-only