Home > Guides, TKJ Electronics > A practical approach to Kalman filter and how to implement it

## A practical approach to Kalman filter and how to implement it

I have for a long time been interrested in Kalman filers and how they work, I also used a Kalman filter for my Balancing robot, but I never explained how it actually was implemented. Actually I had never taken the time to sit down with a pen and a piece of paper and try to do the math by myself, so I actually did not know how it was implemented.
It turned out to be a good thing, as I actually discovered a mistake in the original code, but I will get back to that later.

I actually wrote about the Kalman filter as my master assignment in high school back in December 2011. But I only used the Kalman filter to calculate the true voltage of a DC signal modulated by known Gaussian white noise. My assignment can be found in the following zip file: http://www.tkjelectronics.dk/uploads/Kalman_SRP.zip. It is in danish, but you can properly use google translate to translate some of it. If you got any specific questions regarding the assignment, then ask in the comments below.

Okay, but back to the subject. As I sad I had never taken the time to sit down and do the math regarding the Kalman filter based on an accelerometer and a gyroscope. It was not as hard as I expected, but I must confess that I still have not studied the deeper theory behind, on why it actually works. But for me, and most people out there, I am more interrested in implementing the filter, than in the deeper theory behind and why the equations works.

Before we begin you must have some basic knowledge about matrices like multiplication of matrices and transposing of matrices. If not then please take a look at the following websites:

For those of you who do not know what a Kalman filter is, it is an algorithm which uses a series of measurements observed over time, in this context an accelerometer and a gyroscope. These measurements will contain noise that will contribute to the error of the measurement. The Kalman filter will then try to estimate the state of the system, based on the current and previous states, that tend to be more precise that than the measurements alone.

In this context the problem is that the accelerometer is in general very noise when it is used to measure the gravitational acceleration since the robot is moving back and forth. The problem with the gyro is that it drifts over time – just like a spinning wheel-gyro will start to fall down when it is losing speed.
In short you can say that you can only trust the gyroscope on a short term while you can only trust the accelerometer on a long term.

There is actually a very easy way to deal with this by using a complimentary filter, which basicly just consist of a digital low-pass filter on the accelerometer and digital high-pass filter on the gyroscope readings. But it is not as accurate as the Kalman filter, but other people have succesfully build balancing robots using a fine-tuned complimentary filter.

More information about gyroscopes, accelerometer and complimentary filters can be found in this pdf. A comparison between a complimentary filter and a Kalman filter can be found in the following blog post.

The Kalman filter operates by producing a statistically optimal estimate of the system state based upon the measurement(s). To do this it will need to know the noise of the input to the filter called the measurement noise, but also the noise of the system itself called the process noise. To do this the noise has to be Gaussian distributed and have a mean of zero, luckily for us most random noise have this characteristic.

The system state $\boldsymbol{x}_k$
The next of this article might seem very confusing for some, but I promise you if you grab a pen and a piece of paper and try to follow along it is not that hard if you are reasonable at math.

If you, like me, do not have a calculator or computer program that can work with matrices, then I recommend the free online calculator Wolfram Alpha. I used it for all the calculations in this article.

I will use the same notation as the wikipedia article, but I will like to note that when the matrixes are constants and does not depend on the current time you do not have to write the k after them. So for instance $F_k$ can be simplified to $F$.

Also I would like to write a small explanation of the other aspects of the notations.
First I will make a note about whats called the previous state:

$\boldsymbol{\hat{x}}_{k-1 | k-1}$

Which is the previous estimated state based on the previous state and the estimates of the states before it.

The next is the a priori state:

$\boldsymbol{\hat{x}}_{k | k-1}$

A priori means the estimate of the state matrix at the current time k based on the previous state of the system and the estimates of the states before it.

The last one is called a posteriori state:

$\boldsymbol{\hat{x}}_{k | k}$

Is the estimated of the state at time k given observations up to and including at time k.

The problem is that the system state itself is hidden and can only be observed through observation $z_k$. This is also called a Hidden Markov model.

This means that the state will be based upon the state at time k and all the previous states. That also means that you can not trust the estimate of the state before the Kalman filter has stabilized – take a look at the graph at the front page of my assignment.

The hat over the $\hat{x}$ means that is the estimate of the state. Unlike just a single $x$ which means the true state – the one we are trying to estimate.
So the notation for the state at time k is:

$\boldsymbol{x}_k$

The state of the system at time k if given by:

$\boldsymbol{x}_k = \boldsymbol{F}x_{k-1} + \boldsymbol{B}u_k + w_k$

Where $x_k$ is the state matrix which is given by:

$\boldsymbol{x}_k = \begin{bmatrix} \theta \\ \dot{\theta}_b \end{bmatrix}_k$

As you can see the output of the filter will be the angle $\theta$ but also the bias $\dot{\theta}_b$ based upon the measurements from the accelerometer and gyroscope. The bias is the amount the gyro has drifted. This means that one can get the true rate by subtracting the bias from the gyro measurement.

The next is the $F$ matrix, which is the state transition model which is applied to the prevouis state $x_{k-1}$.

In this case $F$ is defined as:

$\boldsymbol{F} = \begin{bmatrix} 1 & -\Delta t \\ 0 & 1 \end{bmatrix}$

I know that the $-\Delta t$ might seem confusing, but it will make sense later (take a look at my comment).

The next is the control input $u_k$, in this case it is the gyroscope measurement in degrees per second (°/s) at time k, this is also called the rate $\dot{\theta}$. We will actually rewrite the state equation as:

$\boldsymbol{x}_k = \boldsymbol{F}x_{k-1} + \boldsymbol{B}{\dot{\theta}_k} + w_k$

The next thing is the $B$ matrix. Which is called the control-input model, which is defined as:

$\boldsymbol{B} = \begin{bmatrix} \Delta t \\ 0 \end{bmatrix}$

This makes perfectly sense as you will get the angle $\theta$ when you multiply the rate $\dot{\theta}$ by the delta time $\Delta t$ and since we can not calculate the bias directly based on the rate we will set the bottom of the matrix to 0.

$w_k$ is process noise which is Gaussian distributed with a zero mean and with covariance $Q$ to the time k:

$\boldsymbol{w}_k \sim N\left ( 0, \boldsymbol{Q}_k \right )$

$Q_k$ is the process noise covariance matrix and in this case the covariance matrix of the state estimate of the accelerometer and bias. In this case we will consider the estimate of the bias and the accelerometer to be independent, so it’s actually just equal to the variance of the estimate of the accelerometer and bias.
The final matrix is defined as so:

$\boldsymbol{Q}_k = \begin{bmatrix} Q_\theta & 0 \\ 0 & Q_{\dot{\theta}_b} \end{bmatrix}\Delta t$

As you can see the $Q_k$ covariance matrix depends on the current time k, so the accelerometer variance $Q_\theta$ and the variance of the bias $Q_{\dot{\theta}_b}$ is multiplied by the delta time $\Delta t$.
This makes sense as the process noise will be larger as longer time it is since the last update of the state. For instance the gyro could have drifted.
You will have to know these constants for the Kalman filter to work.
Note if you set a larger value, the more noise in the estimation of the state. So for instance if the estimated angle starts to drift you have to increase the value of $Q_{\dot{\theta}_b}$. Otherwise if the estimate tends to be slow you are trusting the estimate of the angle too much and should try to decrease the value of $Q_\theta$ to make it more responsive.

The measurement $\boldsymbol{z}_k$
Now we will take a look at the observation or measurement $z_k$ of the true state $x_k$. The observation $z_k$ is given by:

$\boldsymbol{z}_k = \boldsymbol{H}x_{k} + v_k$

As you can see the measurement $z_k$ is given by the current state $x_k$ multiplied by the $H$ matrix plus the measurement noise $v_k$.

$H$ is called the observation model and is used to map the true state space into the observed space. The true state can not be observed. Since the measurement is just the measurement from the accelerometer, $H$ is given by:

$\boldsymbol{H} = \begin{bmatrix} 1 & 0 \end{bmatrix}$

The noise of the measurement have to be Gaussian distributed as well with a zero mean and $R$ as the covariance:

$\boldsymbol{v}_k \sim N\left ( 0, \boldsymbol{R} \right )$

But as $R$ is not a matrix the measurement noise is just equal to the variance of the measurement, since the covariance of the same variable is equal to the variance. See this page for more information.
Now we can define $R$ as so:

$\boldsymbol{R} = E \begin{bmatrix} v_k & {v_k}^T \end{bmatrix} = var(v_k)$

We will assume that the measurement noise is the same and does not depend on the time k:

$var(v_k) = var(v)$

Note that if you set the measurement noise variance $var(v)$ too high the filter will respond really slowly as it is trusting new measurements less, but if it is too small the value might overshoot and be noisy since we trust the accelerometer measurements too much.

So to round up you have to find the the process noise variances $Q_\theta$ and $Q_{\dot{\theta}_b}$ and the measurement variance of the measurement noise $var(v)$. There are multiple ways to find them, but it is out of the aspect of this article.

The Kalman filter equations
Okay now to the equations we will use to estimate the true state of the system at time k $\hat{x}_k$. Some clever guys came up with equations found below to estimate the state of the system.
The equations can be written more compact, but I prefer to have them stretched out, so it is easier to implement and understand the different steps.

Predict
In the first two equations we will try to predict the current state and the error covariance matrix at time k. First the filter will try to estimate the current state based on all the previous states and the gyro measurement:

$\boldsymbol{\hat{x}}_{k | k-1} = \boldsymbol{F}\hat{x}_{k-1 | k-1} + \boldsymbol{B}{\dot{\theta}_k}$

That is also why it is called a control input, since we use it as an extra input to estimate the state at the current time k called the a priori state $\hat{x}_{k | k-1}$ as described in the beginning of the article.

The next thing is that we will try to estimate the a priori error covariance matrix $P_{k | k-1}$ based on the previous error covariance matrix $P_{k-1 | k-1}$, which is defined as:

$\boldsymbol{P}_{k | k-1} = \boldsymbol{F}\boldsymbol{P}_{k-1 | k-1}\boldsymbol{F}^T + \boldsymbol{Q}_k$

This matrix is used to estimate how much we trust the current values of the estimated state. The smaller the more we trust the current estimated state. The principle of the equation above is actually pretty easy to understand, as it is pretty obvious that the error covariance will increase since we last updated the estimate of the state, therefore we multiplied the error covariance matrix by the state transition model $F$ and the transpose of that $F^T$ and add the current process noise $Q_k$ at time k.

The error covariance matrix $P$ in our case is a 2×2 matrix:

$\boldsymbol{P} = \begin{bmatrix} P_{00} & P_{01} \\ P_{10} & P_{11} \end{bmatrix}$

Update
The fist thing we will do is to compute the difference between the measurement $z_k$ and the a priori state $x_{k | k-1}$, this is also called the innovation:

$\boldsymbol{\tilde{y}}_k = \boldsymbol{z}_k - \boldsymbol{H}\hat{x}_{k | k-1}$

The observation model $H$ is used to map the a priori state $x_{k | k-1}$ into the observed space which is the measurement from the accelerometer, therefore the innovation is not a matrix

$\boldsymbol{\tilde{y}}_k = \begin{bmatrix} \boldsymbol{\tilde{y}} \end{bmatrix}_k$

The next thing we will do is calculate whats called the innovation covariance:

$\boldsymbol{S}_k = \boldsymbol{H} \boldsymbol{P}_{k | k-1} \boldsymbol{H}^T + \boldsymbol{R}$

What it does is that it tries to predict how much we should trust the measurement based on the a priori error covariance matrix $P_{k | k-1}$ and the measurement covariance matrix $R$. The observation model $H$ is used to map the a priori error covariance matrix $P_{k | k-1}$ into observed space.

The bigger the value of the measurement noise the larger the value of $S$, this means that we do not trust the incoming measurement that much.
In this case $S$ is not a matrix and is just written as:

$\boldsymbol{S}_k = \begin{bmatrix} \boldsymbol{S} \end{bmatrix}_k$

The next step is to calculate the Kalman gain. The Kalman gain is used to to indicate how much we trust the innovation and is defined as:

$\boldsymbol{K}_k = \boldsymbol{P}_{k | k-1} \boldsymbol{H}^T \boldsymbol{S}_k^{-1}$

You can see that if we do not trust the innovation that much the innovation covariance $S$ will be high and if we trust the estimate of the state then the error covariance matrix $P$ will be small the Kalman gain will therefore be small and oppesite if we trust the innovation but does not trust the estimation of the current state.

If you take a deeper look you can see that the transpose of the observation model $H$ is used to map the state of the error covariance matrix $P$ into observed space. We then compare the error covariance matrix by multiplying with the inverse of the innovation covariance $S$.

This make sense as we will use the observation model $H$ to extract data from the state error covariance and compare that with the current estimate of the innovation covariance.

Note that if you do not know the state at startup you can set the error covariance matrix like so:

$\boldsymbol{P} = \begin{bmatrix} L & 0 \\ 0 & L \end{bmatrix}$

Where $L$ represent a large number.

For my balancing robot I know the starting angle and I find the bias of the gyro at startup by calibrating, so I assume that the state will be known at startup, so I initialize the error covariance matrix like so:

$\boldsymbol{P} = \begin{bmatrix} 0 & 0 \\ 0 & 0 \end{bmatrix}$

In this case the Kalman gain is a 2×1 matrix:

$\boldsymbol{K} = \begin{bmatrix} K_0 \\ K_1 \end{bmatrix}$

Now we can update the a posteriori estimate of the current state:

$\boldsymbol{\hat{x}}_{k | k} = \boldsymbol{\hat{x}}_{k | k-1} + \boldsymbol{K}_k \; \boldsymbol{\tilde{y}}_k$

This is done by adding the a priori state $\hat{x}_{k | k-1}$ with the Kalman gain multiplied by the innovation $\tilde{y}_k$.
Remember that the innovation $\tilde{y}_k$ is the difference between the measurement $z_k$ and the estimated priori state $\hat{x}_{k | k-1}$, so the innovation can both be positive and negative.

A little simplified the equation can be understood as we simply correct the estimate of the a priori state $\hat{x}_{k | k-1}$, that was calculated using the previous state and the gyro measurement, with the measurement – in this case the accelerometer.

The last thing we will do is update the a posteriori error covariance matrix:

$\boldsymbol{P}_{k | k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}) \boldsymbol{P}_{k | k-1}$

Where $I$ is called the identity matrix and is defined as:

$\boldsymbol{I} = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix}$

What the filter is doing is that it is basically self-correcting the error covariance matrix based on how much we corrected the estimate. This make sense as we corrected the state based the a priori error covariance matrix $P_{k | k-1}$, but also the innovation covariance $S_k$.

Implementing the filter
In this section I will use the equation from above to implement the filter into a simple c++ code that can be used for balancing robots, quadcopters and other applications where you need to compute the angle, bias or rate.

In case you want the code next to you, it can be found at github: https://github.com/TKJElectronics/KalmanFilter.

I will simply write the equations at the top of each step and then simplify them after that I will write how it is can be done i C and finally I will link to calculations done in Wolfram Alpha in the bottom of each step, as I used them to do the calculation.

Step 1:

$\boldsymbol{\hat{x}}_{k | k-1} = \boldsymbol{F}\hat{x}_{k-1 | k-1} + \boldsymbol{B}{\dot{\theta}_k}$

$\begin{bmatrix} \theta \\ \dot{\theta}_b \end{bmatrix}_{k | k-1} = \begin{bmatrix} 1 & -\Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \theta \\ \dot{\theta}_b \end{bmatrix}_{k-1 | k-1} + \begin{bmatrix} \Delta t \\ 0 \end{bmatrix} \dot{\theta}_k$

$= \begin{bmatrix} \theta - \dot{\theta}_b \Delta t \\ \dot{\theta}_b \end{bmatrix}_{k-1 | k-1} + \begin{bmatrix} \Delta t \\ 0 \end{bmatrix} \dot{\theta}_k$

$= \begin{bmatrix} \theta - \dot{\theta}_b \Delta t + \dot{\theta} \Delta t \\ \dot{\theta}_b \end{bmatrix}$

$= \begin{bmatrix} \theta + \Delta t (\dot{\theta} - \dot{\theta}_b) \\ \dot{\theta}_b \end{bmatrix}$

As you can see the a priori estimate of the angle is $\hat{\theta}_{k | k-1}$ is equal to the estimate of the previous state $\hat{\theta}_{k-1 | k-1}$ plus the unbiased rate times the delta time $\Delta t$.
Since we can not directly measure the bias the estimate of the a priori bias is just equal to the previous one.

This can be written in C like so:

rate = newRate - bias;
angle += dt * rate;

Note that I calculate the unbiased rate, so it can be be used by the user as well.

Step 2:

$\boldsymbol{P}_{k | k-1} = \boldsymbol{F}\boldsymbol{P}_{k-1 | k-1}\boldsymbol{F}^T + \boldsymbol{Q}_k$

$\begin{bmatrix} P_{00} & P_{01} \\ P_{10} & P_{11} \end{bmatrix}_{k | k-1} = \begin{bmatrix} 1 & -\Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} P_{00} & P_{01} \\ P_{10} & P_{11} \end{bmatrix}_{k-1 | k-1} \begin{bmatrix} 1 & 0 \\ -\Delta t & 1 \end{bmatrix} + \begin{bmatrix} Q_\theta & 0 \\ 0 & Q_{\dot{\theta}_b} \end{bmatrix}\Delta t$

$= \begin{bmatrix} P_{00} - \Delta t P_{10} & P_{01} - \Delta t P_{11} \\ P_{10} & P_{11} \end{bmatrix}_{k-1 | k-1} \begin{bmatrix} 1 & 0 \\ -\Delta t & 1 \end{bmatrix} + \begin{bmatrix} Q_\theta & 0 \\ 0 & Q_{\dot{\theta}_b} \end{bmatrix}\Delta t$

$= \begin{bmatrix} P_{00} -\Delta t P_{10} -\Delta t (P_{01} - \Delta t P_{11}) & P_{01} -\Delta t P_{11} \\ P_{10} -\Delta t P_{11} & P_{11} \end{bmatrix}_{k-1 | k-1} + \begin{bmatrix} Q_\theta & 0 \\ 0 & Q_{\dot{\theta}_b} \end{bmatrix}\Delta t$

$= \begin{bmatrix} P_{00} -\Delta t P_{10} -\Delta t (P_{01} - \Delta t P_{11}) + Q_\theta \Delta t & P_{01} -\Delta t P_{11} \\ P_{10} -\Delta t P_{11} & P_{11} + Q_{\dot{\theta}_b} \Delta t \end{bmatrix}$

$= \begin{bmatrix} P_{00} + \Delta t (\Delta t P_{11} - P_{01} - P_{10} + Q_\theta) & P_{01} -\Delta t P_{11} \\ P_{10} -\Delta t P_{11} & P_{11} + Q_{\dot{\theta}_b} \Delta t \end{bmatrix}$

The equations above can be written in C like so:

P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_gyroBias * dt;

Note that this is the part of the code that there was an error in in the original code that I used.

Step 3:

$\boldsymbol{\tilde{y}}_k = \boldsymbol{z}_k - \boldsymbol{H}\hat{x}_{k | k-1}$

$= \boldsymbol{z}_k - \begin{bmatrix} 1 & 0 \end{bmatrix} \begin{bmatrix} \theta \\ \dot{\theta}_b \end{bmatrix}_{k | k-1}$

$= \boldsymbol{z}_k - \theta_{k | k-1}$

The innovation can be calculated in C like so:

y = newAngle - angle;

Step 4:

$\boldsymbol{S}_k = \boldsymbol{H} \boldsymbol{P}_{k | k-1} \boldsymbol{H}^T + \boldsymbol{R}$

$= \begin{bmatrix} 1 & 0 \end{bmatrix} \begin{bmatrix} P_{00} & P_{01} \\ P_{10} & P_{11} \end{bmatrix}_{k | k-1} \begin{bmatrix} 1 \\ 0 \end{bmatrix} + \boldsymbol{R}$

$= {P_{00}}_{k | k-1} + \boldsymbol{R}$

$= {P_{00}}_{k | k-1} + var(v)$

Again the C code is pretty simple:

S = P[0][0] + R_measure;

Step 5:

$\boldsymbol{K}_k = \boldsymbol{P}_{k | k-1} \boldsymbol{H}^T \boldsymbol{S}_k^{-1}$

$\begin{bmatrix} K_0 \\ K_1 \end{bmatrix}_k = \begin{bmatrix} P_{00} & P_{01} \\ P_{10} & P_{11} \end{bmatrix}_{k | k-1} \begin{bmatrix} 1 \\ 0 \end{bmatrix} \boldsymbol{S}_k^{-1}$

$= \begin{bmatrix} P_{00} \\ P_{10} \end{bmatrix}_{k | k-1} \boldsymbol{S}_k^{-1}$

$= \dfrac{\begin{bmatrix} P_{00} \\ P_{10}\end{bmatrix}_{k | k-1}}{\boldsymbol{S}_k}$

Note that in other cases $S$ can be a matrix and you can not just simply divide $P$ by $S$. Instead you have to calculate the inverse of the matrix. See the following page for more information on how to do so.

The C implementation looks like this:

K[0] = P[0][0] / S;
K[1] = P[1][0] / S;

Step 6:

$\boldsymbol{\hat{x}}_{k | k} = \boldsymbol{\hat{x}}_{k | k-1} + \boldsymbol{K}_k \; \boldsymbol{\tilde{y}}_k$

$\begin{bmatrix} \theta \\ \dot{\theta}_b \end{bmatrix}_{k | k} = \begin{bmatrix} \theta \\ \dot{\theta}_b \end{bmatrix}_{k | k-1} + \begin{bmatrix} K_0 \\ K_1 \end{bmatrix}_k \boldsymbol{\tilde{y}}_k$

$= \begin{bmatrix} \theta \\ \dot{\theta}_b \end{bmatrix}_{k | k-1} + \begin{bmatrix} K_0 \; \boldsymbol{\tilde{y}} \\ K_1 \; \boldsymbol{\tilde{y}} \end{bmatrix}_k$

Yet again the equation end up pretty short, and can be written as so in C:

angle += K[0] * y;
bias += K[1] * y;

Step 7:

$\boldsymbol{P}_{k | k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}) \boldsymbol{P}_{k | k-1}$

$\begin{bmatrix} P_{00} & P_{01} \\ P_{10} & P_{11} \end{bmatrix}_{k | k} = \left(\begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} - \begin{bmatrix} K_0 \\ K_1 \end{bmatrix}_k \begin{bmatrix} 1 & 0 \end{bmatrix}\right) \begin{bmatrix} P_{00} & P_{01} \\ P_{10} & P_{11} \end{bmatrix}_{k | k-1}$

$= \left(\begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} - \begin{bmatrix} K_0 & 0 \\ K_1 & 0 \end{bmatrix}_k \right) \begin{bmatrix} P_{00} & P_{01} \\ P_{10} & P_{11} \end{bmatrix}_{k | k-1}$

$= \begin{bmatrix} P_{00} & P_{01} \\ P_{10} & P_{11} \end{bmatrix}_{k | k-1} - \begin{bmatrix} K_0 \; P_{00} & K_0 \; P_{01} \\ K_1 \; P_{00} & K_1 \; P_{01} \end{bmatrix}$

Remember that we decrease the error covariance matrix again, since the error of the estimate of the state has been decreased.
The C code looks like this:

float P00_temp = P[0][0];
float P01_temp = P[0][1];

P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;

Note that I have found that the following variances works perfectly for most IMUs:

float Q_angle = 0.001;
float Q_gyroBias = 0.003;
float R_measure = 0.03;

Remember that it’s very important to set the target angle at startup if you need to use the output at startup. For more information, see the calibration routine for my balancing robot.

In case you missed it here is the library I wrote a library that can be used by any microcontroller that supports floating math. The source code can be found at github: https://github.com/TKJElectronics/KalmanFilter.

If you prefer a video explanation about the Kalman filter, I recommend the following video series: http://www.youtube.com/watch?v=FkCT_LV9Syk.

Note that you can not use the library if you need to represent something in a full 3D orientations, as euler angles suffer from what is called Gimbal lock you will need to use Quaternions to do that, but that is a whole nother story. For now take a look at the following page.

This is all for know, I hope that you will find i helpfull, if you do or have any questions fell free to post a comment below – it supports LaTeX syntax as well, if you need to write equations.
If you spot any errors please let me know as well.

Categories: Tags:
1. April 28th, 2015 at 17:21 | #1

Hi Lauszus,

Thank you so much for putting all this together! It helped me a lot.

I have a question. Sorry if other people have asked this before. My system has a periodic jerk input( the acceleration measurement spikes due to that) from knocking into the wall, which mess up the orientation estimation. If the system doesn’t have that jerk it works fine.
From reading your document above I feel like maybe I should adjust my process noise covariance matrix accordingly. Am I on the right track? Also I am using a sampling frequency of 500Hz.

thank you very much for your time

2. May 8th, 2015 at 06:45 | #2

Hi Lasuzus,

Thank you for the filter and all the explanation, my kalman filter is working very well with positive values of my angles from the gyro and the acc but when i try to use the negative values my angle just stays at 0 and after that’s done nothing works until a reset my board, i already checked the angles one by one from the gyro and acc and y get the right negative values, i dont know whats going on, i hope you can help thanks

3. May 10th, 2015 at 20:17 | #3

Hi , i didn’t understand very well this phrase :”Which is the previous estimated state based on the previous state and the estimates of the states before it.” ?? can you simplify plz
thanks

4. May 10th, 2015 at 20:36 | #4

please how can i specifie the process noise covariances , i’m using a MPU-6050 and kalman filter to read value from the gyroscope and accelerometer
thnaks

5. May 11th, 2015 at 01:27 | #5

@klaus

6. June 9th, 2015 at 13:02 | #6

I am still struggling to adapt this to my use though. I’m going wrong somewhere and I’d really appreciate some pointers. I am working on a altimeter.

I have a atmospheric pressure sensor, a BMP180. I want to implement a kalman filter to reduce the noise on readings from this part. Height error (noise) on the BMP180 measurement is 0.25m rms. There is only one input to the filter, height. Rate (velocity) may be calculated by looking at the previous height (filter output) and the current height (measurement including noise) and dividing by dt.
The input to the filter is height in meters, I’d like the output to be height in meters.

The output of the sensor (after conversion from hPa) is in meters. The sensor has internal compensation so bias can be considered to be zero.

Step 0.
P matrix initialised to be 0,0,0,0 as I’m assuming that I know the start height.

Step 1.
Rate = Measured height-Previous height / dt
Height = Measured height

Step 2.
P[0][0] += dt * (dt*P[1][1] -- P[0][1] -- P[1][0] + Q_height);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_gyroBias * dt;

I don’t have a gyro, I’ve only got one input parameter, so Q-gyroBias is zero.

Step 3.
y = Current measurement -- Previous filter output;

Step 4.
S = P[0][0] + R_measure
I’ve set R_measure to be 0.25 to match the rms noise in meters.

Step 5.
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;

Step 6.
Height = Height +K[0]*y
Bias = Bias +K[1]*y

I think Bias will always be zero, but hey, I implemented it.

Step 7.
float P00_temp = P[0][0];
float P01_temp = P[0][1];

P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;

I’ve modelled this with an Excel spreadsheet and the kalman filter doesn’t appear to be working. Any assistance greatly appreciated. It appears that by removing one of the inputs from the filter I’ve confused the heck out of myself!

Many thanks,
Mike Veal.

7. June 9th, 2015 at 13:16 | #7

@Mike Veal
Sorry, but I don’t have a lot of time at the moment, as I am writing my bachelor’s thesis.

But have you considered using a accelerometer as well? All the code I have seen use the accelerometer z-axis in the combination with a barometer in order to estimate the height.

Note that I might be working on this next semester for my flight controller. I don’t know what your timeframe is, but you can following my Github repository: https://github.com/Lauszus/LaunchPadFlightController where the code will be posted.

8. June 9th, 2015 at 13:28 | #8

@ Kristian
Sadly cost is a driver as are size and power. The device is to be used on a paraglider and is battery powered.
I need to have a filter running in the next month -- then it’s off to Solvenia for a week of intensive altimeter testing!

Best of luck with the thesis.

If any one else is able to help I’d greatly appreciate it.

9. June 9th, 2015 at 13:31 | #9

@Mike Veal
But a MPU-6050 is less than \$3 on eBay, so I can’t see no reason for not using it?

Also a Kalman filter makes no sense if you are only using one sensor! That is why it is called a sensor fusion algorithm.

10. June 10th, 2015 at 12:09 | #10

Perhaps that’s what I’m struggling to understand. I can find no simplification examples with just one input.
Adding an accerometer /gyro would also make things much more complicated.
What I really need is two outputs, height and rate of change of height.
A 3 axis accelerometer / gyro could give me that infomation, but the glider will always have some forward speed as well as vertical speed.
Due to pitch, roll, yaw and repeatability of mounting axis, glider forwards motion will never be exactly on the X axis and vertical motion will never be exactly in the Z axis.
I’d have to try and separate a glider forward motion vector from the sensor output so that I could work out what constitutes vertical movement.

It sounds like a simple low pass filter would be easiest.

11. June 10th, 2015 at 17:20 | #11

@Mike Veal
That can be compensated for by rotating the coordinate system, as I do for my flight controller as well: https://github.com/Lauszus/LaunchPadFlightController/blob/master/src/IMU.c. Then you can just use the filtered (in this case ‘accFiltered’) z-axis and integrate that.

Yes if you only want to use the barometer, then a low pass filter seems like the way to go.

12. August 1st, 2015 at 05:38 | #12

Hi, nice article,

have you comparated the results from the Kalman filter and a complementary filter?, y did see the link to a web where they do that but the graphics dont look good.
I have made a comparison and with the parameters that I have used the Kalman filter and the complementary filters are practically the same, however I made a test, where I moved the IMU linearly in each axis and with the Kalman filter there was a variation of about +/-10 degrees but with the complementary filter the variation was a lot less (about +/- 2 deg).
Despite that I got similar results using both filters I dont know if I can improve the Kalman filter.

13. August 3rd, 2015 at 00:20 | #13

@Fabian
No not really, but to be honest I am starting to use a complimentery based filter more and more, as it is just much easier to tune, as takes less time to compute. For instance I have been using this lately with my flight controller: https://github.com/Lauszus/LaunchPadFlightController/blob/master/src/IMU.c.

14. August 10th, 2015 at 16:14 | #14

Hi! I have a question about the implementation step 1. In this step you said that the apriori estimate of the angle was equal to the estimate of the previous state plus the unbiased rate times the delta time. The issue I am dwelling on is the the unbiased rate itself. This rate seems to be a diference between the rate at time k and the bias at time k-1|k-1. Is is permissible to subtract two values obtained from different time levels?

Konstantin

P.S.: The expression in the brakets in the last row, step 1, would be

unbiased rate = theta_dot,k -- theta_dot_bias,k-1|k-1

if you didn’t omit the indeces.

15. September 3rd, 2015 at 00:50 | #15

Hi, I’ve been checking your code about kalman filter, and I’m wondering why you divide by 131.0:

double gyroZrate = gyroZ / 131.0; // Convert to deg/s

how can I get my own value?

16. September 7th, 2015 at 12:23 | #16

@Erick Medina
It has to do with the so-called Sensitivity Scale Factor. Check the following document, page 12:
MPU-6000 and MPU-6050 Product Specification Revision 3.3
The factor is 131 LSB/(deg/s) which means that you must divide the raw data to 131 in order to get real angular velocity.

17. September 9th, 2015 at 08:11 | #17

How do you compare the HX (which is the angle estimate) with z (which is the accel measurement)?

Is this an approximation of some kind, and if so, what is the actual equation from which it is derived?
In the case of this being an approximation, it will only work for balancing type scenarios (small deflections), right?

18. September 9th, 2015 at 08:45 | #18

@Sander
Hi,

I have the exact doubt. Did you manage to find an explanation for it?

19. September 9th, 2015 at 15:08 | #19

Hi,

First of all I must say that you wrote an excellent atricle. Now let me brief you my problem , I want to estimate heading for a vechile with using IMU . I am using Murata SCC1300-D02 sensor , it has
3axis accerolmeter and 1-axis gyrometer. I am not using any magnometer so my question is that is it possible to estimate the heading(orientation) of the Vechile with the above mentioned sensor and using Kalman filter which you have written?

20. September 27th, 2015 at 05:32 | #20

Nicely done. Found one typo: “control-input model” should be “control-input matrix”.

21. September 30th, 2015 at 14:09 | #21

No that will be very difficult. You didn’t say what axis the gyro were measuring, but I guess it is the z-axis. Anyway I will not recommend this, as the integration will integrate noise as well.

You might want to take a look on how I did it for my flight controller: http://blog.tkjelectronics.dk/2015/08/bachelors-thesis-launchpad-flight-controller. The relevant code is in here: https://github.com/Lauszus/LaunchPadFlightController/blob/master/src/IMU.c.

@Law
Not according to Wiki: https://en.wikipedia.org/wiki/Kalman_filter#Underlying_dynamic_system_model, but thanks for your feedback anyway!

22. January 6th, 2016 at 08:24 | #22

Hi..excellent article..thanks a ton.
I?ve been trying to estimate altitude from barometer(measured) and accelerometer(predicts and also gravity removed somehow)..so how do I find the covariance matrix considering if I know the error in accelerometer..how do I find error in position and velocity.?

23. January 15th, 2016 at 09:22 | #24

@Kristian Sloth Lauszus
Hi..may I ask..How did you came up with the idea of using (theta dot b) as second state variable?

24. January 18th, 2016 at 08:46 | #25

@Kristian Sloth Lauszus

Hi, I have one question regarding the angle. In your code you have mentioned an argument as new angle. Can you tell me what is new angle representing here?

Best Regards,
Umair

25. January 18th, 2016 at 14:50 | #26

I saw some other code where they did that as well.

newAngle is the angle measured using the accelerometer.

26. January 21st, 2016 at 23:58 | #27

The derivation is missing a state variable (theta dot) and the rate gyro measurement should be used as a measurement, not as a “control variable”.

A more complete discussion of the problem is given in http://home.earthlink.net/~schultdw/bbot/bbot.html

27. February 1st, 2016 at 21:06 | #28

@Jim Remington
I know that the article has some shortcomings, but I don’t have time to write a new one at the moment. But remember that I wrote this article back in High School before I was actually formally taught any of this stuff.

28. February 25th, 2016 at 09:53 | #29

and here’s another good source for learning Kalman

29. February 25th, 2016 at 10:06 | #30

30. March 1st, 2016 at 09:52 | #31

Hi Kristian,

Nice article. I am interested to use kalman filter to my following application can u suggest which code i use.
1. My application is to measure Tilt angle in X & Y direction +/- 35 degrees. I use analog
device ADIS16209 (Inclinometer & accelerometer) with ADIS16265 (digital gyroscope). I interested to measure both X & Y direction accurate tilt angle in dynamic condition also.

2. Presently I measure accurate tilt angle in static condition without gyroscope.

31. March 12th, 2016 at 09:09 | #32

Hey..quick question..When we write

Yk= Zk- (H * Xk)

which is basically subtraction between measured value and predicted value.
we set H matrix equals as 1 for angle and 0 for bias. That implies no prediction for the Gyro bias via the model and No measurement for Gyro bias too.

But when it comes to updating the predicted states i.e

New_angle= Predicted_angle + Kalman_gain * (Innovation)

we are updating Gyro Bias

GyroBias_est= GyroBias_est + Kalman_gain * Innovation

so same innovation is used for getting new estimates for angle and bias.
I know its correct..since with the help of measured value, it will try to correct Gyro_bias..but it still seems odd..we updated bias with the innovation calculated for angle..!!

32. June 10th, 2016 at 01:49 | #33

In the process noise covariance matrix, Qk, I can understand why Q_angle gets multiplied by dt since the state you’re estimating is the angle, but I do not understand why Q_gyroBias gets multiplied by dt when the state being estimated is the gyro bias, which if I understand correctly, is an angular velocity. Can you clarify this? Can you also clarify if Q_angle is the covariance matrix of the gyroscope measurement and not the accelerometer measurement?

Thanks

33. July 4th, 2016 at 16:56 | #34

Awesome tutorial! helped me a lot.. thanks dude!

34. July 9th, 2016 at 16:15 | #35

Hei, one question, I’m on a project that receive data from 3 GPS’s and I’m being use a kalman filter to estimate a better location for my quadcopter..

About R matrix and Q matrix, How can I get these values ? In the diagonal matrix Q I put the measures from either GPS ?…

Thank you!!

35. August 27th, 2016 at 09:26 | #36

@Fabian
I have tried it and I’ve found out that complementary filter and kalman filter results are almost the same!

I wonder if it has anything to do with the tuning..

36. August 27th, 2016 at 11:37 | #37

@Abdullah Kahraman
A Kalman filter with constant gain (not extended Kalman filter) results in the same as a complimentary filter. The difference is that the Kalman filter helps with the tuning/frequency selection from a model-based description of the system to be measured.

37. August 29th, 2016 at 03:58 | #38

Wow….this is the best explanation of the Kalman Filter I have ever read! Thank you very much! The KF is something I have always wanted to understand without having to know the science. I will be working to port this over to a Intel Curie using Arduino.

FYI -- for some reason I cannot get your Arduino code to compile. The following error pops up on the I2C.ino

exit status 1
‘While’ was not declared in this scope

I will share the Curie code on git once I get it completed.

38. August 31st, 2016 at 23:58 | #39

@Kristian Sloth Lauszus @Thomas Jespersen
Hi, I have one question about the newAngle measured using the accelerometer.

when I put the phone on a table, and rotate it around the z-axis of gyro.
what I want is to calculate the angle of rotating.

I can get the gyroRate from the z-axis of gyro.
But I don’t know what is the newAngle in this case. I mean I don’t know the newAngle is between which two things(lines or axises)?

Thank you!

39. September 8th, 2016 at 14:58 | #40

Hi,
I am an engineer from taiwan.
I see the web have a lot of good stuff.
and I want to ask is there a accelerometer sensor measured in wheel revolutions per second.
I find a lot of web site but I don’t find such like here very professional.

I use the BMA250E accelerometer sensor and mounted on bicycle wheel hub for measured in wheel revolutions per second.
But I got a lot of noisy sine wave and try to implement Kalman on project.

40. September 8th, 2016 at 21:05 | #41

@Mark Ingle
This sounds strange, as it is simply just a while loop?

@jiayi xin
You can not measure the yaw (rotation around the z-axis) using an accelerometer. You will need to use a magnetometer.

@Vincent Kuo
Why not use a simple encoder?

41. September 15th, 2016 at 16:53 | #42

@Vincent Kuo
Of course you will get a lot of sine waves on the measurements -- this is not noise but your actual movement when you rotate an accelerometer placed in the center of a wheel.
As Kristian says it might be better to install some kind of encoder. Maybe a hall effect encoder can be installed depending on the velocity resolution of interest.

42. September 30th, 2016 at 03:54 | #43

I studied Kalman Filter for a whole year during my final year undergrad project in 2011 but reading your blog feels like I just grasped the whole concept clearly. I am now working with IMUs. I might follow your blog to calculate degrees precisely. I’ll probably cite your GitHub project in my work.
Great work! I hope you become a teacher some day. Very few people can clarify difficult concepts this easily.

43. October 10th, 2016 at 20:28 | #44

@Shamir Alavi
Thank you for your kind words!

44. October 17th, 2016 at 08:10 | #45

@Lauszus
if the 15 degree/s is negative ? What would happend?

45. October 17th, 2016 at 09:02 | #46

I realised some error ,but maybe I m wrong .for example unity is inconsistent for Q matrix ,also Q bias has to be dps unity and Q angle vektor is degree in this case Qk matris should multiply [1 deltaT]'(transpose ) and also gyro data can take negative values in this case bias value by status reducing or increasing .
as a result for P matris : I met again unity problem maybe is not important but Im obsession for it.Am I wrong .Also I wonder if I wanna control with a command in this case I should use a +control value in equality (Uk matrix) that will give extra angle to my device .Am I right? Thanks to reply.

46. October 17th, 2016 at 11:03 | #47

A should be [1 -1;0 1] , because b is angle but you are getting like dps and multiply deltaT give degree(bk = b(k-1) +K1(zk -- H*Teta) ) .And As far as I see because of -1 ,b value will decrease our angle and less bias or no bias I dont know Im didn’t try yet.And with bias += K[1]*y , b is fixing its seld as -- or + . Am I right .

47. October 19th, 2016 at 13:47 | #48

Your link to the “PDF” mentioned in your blog seems to be broken? I tried to click on the one for “more information on complimentary filters,” but all I got was a 404 error from an MIT webpage…

48. October 19th, 2016 at 16:24 | #49

Hello Kristian, just wanted to say that I’m impressed by your explanation of the filter, and also the implementation notes proved highly useful for my current project. Actually I understood more your explanation that some specialiced books I’ve been checking. Also, using the angle and its bias as states, only linearly related to the sampling period was brilliantly simple.

So I wonder, if I wanted to further my understanding and / or go beyond, for example to optimal control, what bibliography do you recommend? What did you use to understand this so well?

Thanks a lot.

49. October 31st, 2016 at 07:13 | #50

great article , but i want to ask about if you have eny reffrencies of implementing kalman filter with two measred data ( two diffrent sensors ) and exact data ???

Comment pages
1 4 5 6 7 2868