0: Modelling the system (noisy variables)

Course index:

The most important step in a Kalman filter is to model your system. No matter how fancy your NewAwesome Kalman filter is, it will not do miracles if you do not use the appropriate model for your actual system. One of the secrets for a good performance of the filter is to have a solid and realistic model. Note that I have not said complex, some people confuse realistic with complexity by adding infinite series of parameters and terms. To model a system consists of two steps. Firstly, we describe in a mathematical way the states of interest. Secondly, we determine how these states evolve over time, i.e. their dynamics. In this post we are going to focus on the first step.

During this post we will consider a typical Inertial Measurement Unit (IMU) as a system. In particular, we will see how to model it in order to calibrate its accelerometers. For example, we describe the states of our system by a vector consisting of the IMU’s position, IMU’s velocity and the bias of the IMU’s accelerometer

x=\begin{bmatrix}p_x & v_x & b_x \end{bmatrix}^T. \quad (1)

We will only focus our attention on one of the three axes XYZ, e.g. the ‘x-axis’, since the following reasoning is identical for the three of them by symmetry arguments. Therefore we have that p_x, v_k, b_x\in\mathbb{R} and x\in\mathbb{R}^3. The main objective in this example is to model the system such that it allows us to calibrate the bias of the accelerometers in an autonomous way by employing the Kalman filter.

In order to be realistic, we will consider that our state variables are stochastic, i.e. they are random but some values are more likely to happen than others. In fact, we will assign a probability to each value of p, v and b (for the sake of clarity we drop the subindex x) in the set of the real numbers. One of the most popular models for assigning such probabilities is the called normal or Gaussian distribution, namely

f(p) = \frac{1}{2\pi \sigma_p^2} e^{-\left(\frac{p-\hat p}{\sqrt{2}\sigma_p}\right)^2}, \quad (3)

where \hat p\in\mathbb{R} is the most probable value where the position p is about (also called the mean), and \sigma_p\in\mathbb{R} determines how distributed is the probability about the mean (also called the standard deviation). Therefore, the best guess for a variable described with (3) is to give both, the mean \hat p and its standard deviation \sigma_p. It does not make sense to say the position is at 3m, but to say, the position is around 3m \pm0.5m with a confidence of 60\%.

The position of the IMU is not represented by a number but by a distribution. If one employs (3), then the best guess for the position is around \hat p (vertical line) with a width \pm\sigma_p (horizontal lane).

We also call f(p) the probability density function. What does it mean? since we have a noisy system, what is the probability of having the IMU at the exact position p=0.0000000\dots 0000 \dots? the answer is exactly zero. We have uncertainty in our system, so it is not possible to give a perfect accurate guess for the position of the IMU. This is how mathematically we write the probability of finding the IMU between the positions p_1 and p_2

Prob \, p\in[p_1, p_2] := \int_{p_1}^{p_2} f(p) dp, \quad (4)

therefore the probability of finding precisely the IMU between 0 and 0 is zero. Note that with (3) the probability of finding your IMU between Alpha Centauri and the Sun is not precisely 100\%, but very close to :P. By employing (3), the probability is 100\% if and only if p_1=-\infty and p_2=+\infty. Maybe this sounds  weird, but we should remind that this is just a model and function (3) is not the only way of how to describe a random variable.

It is very convenient to deal with Gaussian state variables. For example, the multiplication of two Gaussian distributions f_1 and f_2 as in (3) gives us again another Gaussian distribution, namely

\frac{1}{2\pi \sigma_{p_1}^2} e^{-\left(\frac{p-\hat p_1}{\sqrt{2}\sigma_{p_1}}\right)^2}\frac{1}{2\pi \sigma_{p_2}^2} e^{-\left(\frac{p-\hat p_2}{\sqrt{2}\sigma_{p_2}}\right)^2} =\frac{1}{2\pi \sigma_{p_f}^2} e^{-\left(\frac{p-\hat p_f}{\sqrt{2}\sigma_{p_f}}\right)^2}, \quad (5)

where \hat p_f = \frac{\hat p_1\sigma_{p_2}^2 +\hat p_2\sigma_{p_1}^2}{\sigma_{p_1}^2 +\sigma_{p_2}^2} and \sigma_{p_f}^2=\frac{\sigma_{p_1}^2 \sigma_{p_2}^2}{\sigma_{p_1}^2+\sigma_{p_2}^2}.

This is the key property for the low computational cost of the Kalman filter with Gaussian variables. For example, if f_1(p) says: the probability of finding the IMU between 1m and 2m is 30\% and f_2(p) says: the probability of finding the IMU between 1m and 2m is 45\%, what is the probability of having both f_1(p) and f_2(p) right? 0.30 \cdot 0.45 = 0.135. Here, if you have not noticed it, we have introduced a notion of sensor fusion, this is what we have done in (5). This is the meaning of p_f and \sigma_{p_f}, we have fused the information from f_1 and f_2, e.g. from two different GPS. Computationally and mathematically speaking, it is very easy to fuse Gaussian information from different sources! In general for other kind of distributions different from (3), this is not the case!

How to model the IMU’s accelerometer?

We already have enough material for modeling our accelerometer. We consider that the readings from the accelerometer can be modeled as a Gaussian variable

a_r = a + f_a,

where a_r, a\in\mathbb{R} are the reading value and the actual value of the acceleration respectively, and f_a has \hat a as the bias, since it determines how much shifted is f_a from zero and \sigma_a  determines how noisy is the sensor, since it determines the shape of f_a. Note that if the IMU is at rest on the table, then a = 0 but a_r \neq 0 in general. The values for the standard deviation and the bias of the accelerometer can be found in the corresponding datasheet, usually at the last pages of the document. The bias and the standard deviation usually depends heavily on the temperature of the sensor.

Now that we know how to model our state variables, let us proceed to the next post, where we will show how to model the dynamics of the states and how to employ the Kalman filter by understanding the physics/maths behind.