• Kalman filter ** Theoretical background *** LTI system The implementation of the code is based on the tutorial from [[https://www.kalmanfilter.net][this site]].

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

[[file:figures/lti-system.png]]

The corresponding discrete form is written as:

[[file:figures/lti-system-discrete.png]]

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

[[file:figures/continuous-to-discrete.png]]

The measurement of the system is given by:

[[file:figures/measurement-of-lti-system.png]]

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

[[file:figures/lti-system-with-uncertainty.png]]

*** 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 [[file:figures/dt.png]], 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 | |-------+----------+--------------------------------------------------------------------------------------------| | 0 | 0 | initial estimation of [[file:figures/x00.png]] and [[file:figures/P00.png]] | | 1 | 0 | /predict:/ make predictions of [[file:figures/x10.png]] and [[file:figures/P10.png]] | |-------+----------+--------------------------------------------------------------------------------------------| | 2 | 1 | get the measured system output [[file:figures/z1.png]] and its unceratinty [[file:figures/R1.png]] | | 3 | 1 | /update:/ update current state [[file:figures/x11.png]] and unceratinty [[file:figures/P11.png]] | | 4 | 1 | /predict:/ make predictions of [[file:figures/x21.png]] and [[file:figures/P21.png]] | |-------+----------+--------------------------------------------------------------------------------------------| | 5 | 2 | get the measured system output [[file:figures/z2.png]] and its unceratinty [[file:figures/R2.png]] | | 6 | 2 | /update:/ update current state [[file:figures/x22.png]] and unceratinty [[file:figures/P22.png]] | | 7 | 2 | /predict:/ make predictions of [[file:figures/x32.png]] and [[file:figures/P32.png]] | |-------+----------+--------------------------------------------------------------------------------------------| | ... | ... | ... | |-------+----------+--------------------------------------------------------------------------------------------| | 3n-1 | n | get the measured system output [[file:figures/zn.png]] and its unceratinty [[file:figures/Rn.png]] | | 3n | n | /update:/ update current state [[file:figures/xnn.png]] and unceratinty [[file:figures/Pnn.png]] | | 3n+1 | n | /predict:/ make predictions of [[file:figures/xnp1n.png]] and [[file:figures/Pnp1n.png]] | |-------+----------+--------------------------------------------------------------------------------------------| | ... | ... | ... |

The corresponding equations for the kalman filter are listed below.

• update:

[[file:figures/update.png]]

• predict:

[[file:figures/predict.png]]

Note that the last equation in the prediction procedure can also be written as: [[file:figures/simplified-corvariance-update.png]]

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: #+begin_src python kalman_filter = Kalman(F, G, H) #+end_src or #+begin_src python kalman_filter = Kalman() kalman_filter.F = F kalman_filter.G = G kalman_filter.H = H #+end_src 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: #+begin_src python kalman_filter.x = x kalman_filter.P = P #+end_src

*** 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: #+begin_src python def predict(self, u: npt.ArrayLike | None = None, Q: npt.ArrayLike | None = None ) -> Kalman: ... #+end_src 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: #+begin_src python def update(self, z: npt.NDArray, R: npt.NDArray | None = None ) -> Kalman: ... #+end_src 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 |

** Examples Numerical examples concerning the Kalman filter are given in examples/examples.py. Here are some snapshots.

• The temperature of heating liquid [[file:examples/example8-1.svg]] [[file:examples/example8-2.svg]]

• The position of a moving vechicle [[file:examples/example9.svg]]

• The altitude of a rocket: [[file:examples/example10-1.svg]] [[file:examples/example10-2.svg]]