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.
For more information about the theory behind the filter take a look at the following pages:

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:


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)

More information about covariance can be found on Wikipedia and in my assignment.

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.

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}

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}

Take a look at my calibration routine for more information.

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.

Wolfram Alpha links:

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.

Wolfram Alpha links:

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;

Wolfram Alpha links:

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;

Wolfram Alpha links:

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;

Wolfram Alpha links:

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;

Wolfram Alpha links:

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: Guides, TKJ Electronics Tags:
  1. faadhil
    May 5th, 2014 at 01:26 | #1

    hi Lauszus

    first of all thank you very much about your explanation about thw Kalman filter, finally i undesrstand almost parts of the algorithm except the covariances but that is out of my concern right now.
    now i am learning your code for ITG3205_ADXL345 IMU, and i have several questons.
    1. what the meaning of this code
    double gyroYrate = (((double)readGyroY() -- zeroValue[4]) / 14.375);
    gyroYangle += gyroYrate * ((double)(micros() -- timer) / 1000000);
    is this the integral function? and why for the gyroXrate you add the sign (-) in the code

    2.int readGyroX() { // This really measures the y-axis of the gyro
    why readGyroX meassures the y-axis why not x-axis?


  2. faadhil
    May 5th, 2014 at 02:44 | #2

    ok Lauszus it seem you and thomas had answered the simmilar questions like mine, sorry i’ve just read all the comments.
    you know my project is to get the (yaw) orientation and position of tracked vehicle robot using 9 DOF IMU and rotary encoder with Kalman filter, so it try to combine the acclero and rotary encoder to get position, and combining the gyro and magneto to get yaw. so what do you think, do i need to use Kalman separately? cos i thought it will be easier to create the matrixs and the states if it is used separately. do you have any example how to combine gyro with magnetometer to get yaw in kalman filter? or maybe some sources? or i can edit the code for ITG3205_ADXL345 IMU may be by changing the measurement from accelero data with magneto data,can’t i?

    Thanks again i really really appreciate your help


  3. Jesse
    June 10th, 2014 at 15:46 | #3

    Hi Lauszus,

    I’m working on a full-scale “Segway” for a project and I’m testing the Kalman as an alternative to a complementary filter. I’ve implemented your code, but I’m finding that the output is a order of magnitude too small (accel is reading 18 degrees and filter is outputting 1.8). This is likely due to a simple misunderstanding of concept on my part, but any suggestions you have would be greatly appreciated!


  4. June 11th, 2014 at 16:35 | #4

    1. Simply don’t add PI at these lines: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU/ITG3205_ADXL345/ITG3205_ADXL345.ino#L117 and https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU/ITG3205_ADXL345/ITG3205_ADXL345.ino#L123.
    2. Simply put it on one of it’s side. One of the axis should then read 1g while the others should read 0g. If they don’t this will be your zero value. Repeat this until you have this behavior on all three axis.

    1. Please read: http://arduino.cc/forum/index.php/topic,58048.0.html where I have described what is going on.
    2. Because the gyro and accelerometer are not correctly orientated on the PCB, so you will have to compensate for this in the code.

    Please see the following example: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU/MPU6050_HMC5883L.

    I am actually build one as well. I will write a blog post about once it is finished.
    The code is available here: https://github.com/Lauszus/BalancingRobotFullSize and it is already balancing as we speak ๐Ÿ˜‰

  5. dsl
    July 2nd, 2014 at 22:28 | #5

    Hi, I think there’s a small bug in your implementation of step 7. On lines 75 and 76 you refer to the priori error covariance matrix (computer in lines 73 and 74) -- but elements P[0][0] and P[0][1] have already been updated to posteriori state. Could you double check? I could be reading this wrong.

    Otherwise, this is super helpful. Great read, thanks.

  6. klaus
    July 10th, 2014 at 12:02 | #6

    hi, i have simulated the whole thing with matlab. if i “turn” my virtual object (changing the gyro and acc values) the estimated angle is correct, but the estimated drift is totally wrong. did you watch the drift estimation in your model? i think there is a failure in the model?

  7. Manh Hoang
    July 12th, 2014 at 10:26 | #7

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

    I can not understand how we can write this from the matrix. Could you please explain more?

    Thanks in advance.

  8. July 14th, 2014 at 12:30 | #8

    You are absolutely correct. I will update it soon -- I moving to San Francisco to study the next semester, so haven’t got much time right now.

    I just tried to output the unbiased rate using getRate() and it looks alright to me. Could you try to do it in hardware and check as well?

    @Manh Hoang
    They two lines were swapped. Please have a look now.

  9. klaus
    July 14th, 2014 at 14:52 | #9

    Please have a look at my simulation http://www.mikrocontroller.net/topic/338563
    Maybe you detect an error in my simulation?

  10. Douglas Santana
    July 17th, 2014 at 15:17 | #10

    Hi Lauszus! In the definition of “Qk” where you say “accelerometer noise”, i think you should say “gyroscope noise”. Please, take a view on it. Congratulations, nice article!

  11. Thamanoon
    July 29th, 2014 at 02:57 | #11

    Thank you for your explanation. I implement your kalman filter in my stm32f3discovery board, it’s work but I don’t know it’s good way or not.

  12. Robert
    July 31st, 2014 at 10:22 | #12

    Hi Lauszus, thanks for this great practical tutorial! I implemented your code succesfully. For anyone, who is interested in how Kalman filter equations are derived in a more or less intuitive manner, then this paper is a great source to begin with: http://www.cl.cam.ac.uk/~rmf25/papers/Understanding%20the%20Basis%20of%20the%20Kalman%20Filter.pdf
    Again, thanks Lauszus!

  13. xeonqq
    August 7th, 2014 at 15:30 | #13

    Hi, Lauszus
    I used your kalman filter code also for my MPU-6050
    I printed out the angles from the serial port, and I compare the plot of raw angle from Acc and angle from complementary filter and kalman filter. And I find out kalman filter has a relative big delay in time than complementary filter , do you know what parameters influence the delay of kalman filter?
    plot is as follows:

  14. Rocky
    August 31st, 2014 at 10:35 | #14

    Since the angle \theta calculated by accelerometer has no bias, why should we subtract gyro bias in the system equation? What ‘s more, these are two independent sensor.

  15. soodabeh
    September 1st, 2014 at 08:15 | #15

    thank you for ur perfect article dear Lauszus ๐Ÿ™‚
    Can I implement it for java and android sets?
    I mean that sensors are same as robots?
    did u see this code for java?

    thank you again ๐Ÿ™‚

  16. Mansuk, Yang
    September 2nd, 2014 at 14:53 | #16

    First of all, I would like to express deep thanks to you. I tested MPU6050_HMC5883L.ino with GY-68. Pitch and roll looks like working correctly but yaw looks like not working.

    And also there are something I could not understand in updateMPU6050 routine:

    void updateMPU6050() {
      while (i2cRead(MPU6050, 0x3B, i2cData, 14)); // Get accelerometer and gyroscope values
      accX = ((i2cData[0] << 8) | i2cData[1]);
      accY = -((i2cData[2] << 8) | i2cData[3]);
      accZ = ((i2cData[4] << 8) | i2cData[5]);
      tempRaw = (i2cData[6] << 8) | i2cData[7];
      gyroX = -(i2cData[8] << 8) | i2cData[9];
      gyroY = (i2cData[10] << 8) | i2cData[11];
      gyroZ = -(i2cData[12] << 8) | i2cData[13];

    gryroX and gyroZ have -- sign in just upper part. Is it correct?

    And also I changed double to int for accX, accY, accZ, gyroX, gyroY, gyroZ, then it does not work.

  17. ary kusuma ningsih
    September 6th, 2014 at 08:42 | #17

    Hi Lauszus! Thank you so much for the explanations! It helps me a lot but still I have to do more paper research regarding this topic. Have a nice day ๐Ÿ™‚

  18. Autorobot
    September 16th, 2014 at 20:44 | #18


    Thanks a lot for the contribution.

    Is there any way to get Extended Kalman Filter ?

    Best Regards,

  19. September 17th, 2014 at 06:42 | #19

    @Douglas Santana
    No I don’t say that. I say that it is: “$latex Q_k$ is the process noise covariance matrix”.

    Please take a look at the following examples: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter.

    You are welcome! And thank you for that link to that article!

    What code are you using? Are you using my code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU/MPU6050?

    Please see the following comment: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/comment-page-1/#comment-57783.

    No I have not tried this code in Java. Instead I have been using the following code for the Android application for the Balanduino: https://github.com/TKJElectronics/BalanduinoAndroidApp/blob/master/src/com/tkjelectronics/balanduino/SensorFusion.java. Source: http://www.thousand-thoughts.com/2012/03/android-sensor-fusion-tutorial/.

    @Mansuk, Yang
    This is because of the way the sensors were orientated. Please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf and http://www.freescale.com/files/sensors/doc/app_note/AN4248.pdf.

    @ary kusuma ningsih
    You are welcome! ๐Ÿ™‚

    I have not looked into that, sorry.

  20. sammy
    September 22nd, 2014 at 22:54 | #20

    hey i have tried implementing kalman filter on IMU called GY-80 using your library but i havent got
    any output but i have got errors.It would be grateful if u could find time to do it for me else please provide me with sensor fusion code on L3G4200D AND ADXL 345. WAITING FOR YOUR CODE OR REPLY

  21. September 22nd, 2014 at 22:58 | #21

    You should have a look at the following code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU/ITG3205_ADXL345/ITG3205_ADXL345.ino. You simply need to tweak the gyro code, so it matches the L3G4200D instead.

  22. Nir
    September 23rd, 2014 at 12:09 | #22

    Hi Lauszus
    Very interesting to read, I would like to build such a balancing robot, but seems like the 6DOF IMU you used is no longer available from some stores, how about other like MPU-6050?
    It also has 3-axis gyroscope and a 3-axis accelerometer, it there something I am missing?

    thanks a lot,

  23. September 23rd, 2014 at 18:45 | #23

    You can differently use the MPU-6050. This is the sensor I have been using the most. It’s also the sensor used for the Balanduino: https://github.com/tkjelectronics/Balanduino#hardware. Also check out this example code: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU/MPU6050.

  24. Rich
    September 30th, 2014 at 19:01 | #24

    This is a great tutorial! I have it working with my IMU now. I don’t undertsand what these values represent:

    const double Q_angle = 0.001;
    const double Q_gyroBias = 0.003;
    const double R_angle = 0.03;

    I have tried different values to get the filter response I need but I would appreciate an explanation of what they are.

    Also, I initialize the filter with setAngle. What value should I use?



  25. October 2nd, 2014 at 00:41 | #25

    Please read the post again. It already explains it.

    I would recommend taking a look at the following example: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/blob/master/IMU/MPU6050/MPU6050.ino if you want to see how to use it in practice.

  26. gaou
    October 3rd, 2014 at 17:16 | #26

    I understood how kalman filters works thanks to you.
    I am not good at reading English.But your?intelligible explanation makes me?read this to the end.Thanks.

    Now,I have a question.
    The theta dot in the equation of step1 gives me a feeling of something wrong.
    Because X(k|k-1) in the left side of the formula should be based on the previous state or estimation.
    The theta dot is a previous data.
    I think the theta dot should not affect the X(k|k-1).
    Could you explain about it?


  27. Rich
    October 3rd, 2014 at 22:22 | #27

    I see the references now. Thank you

  28. G
    October 20th, 2014 at 01:52 | #28

    Tak for det.

    Question: Tracking the bias is done because the bias drifts slowly, yes? That is, you could make this a 1-d problem by just tracking theta and not the bias, but with constant error covariances, P and K would quickly stabilize and you’d end up with essentially a complementary filter.

    I gather that by tracking bias, which varies slowly in comparison to ‘noise’, a more accurate filter is obtained.

    Ikke ogsรฅ?

  29. iftikhar jatoi
    November 10th, 2014 at 08:43 | #29

    dear sir,
    i hve download kalman filter library and copy paste it to arduino library folder.i add the library by correct method of library adding.but this library is not shoeing in examples section of arduino id.
    i think this aws notlibrary file and it was text file having kalman filter code.i downloded it from link
    please anwer!

  30. November 10th, 2014 at 22:07 | #30

    @iftikhar jatoi
    Please see this guide: https://learn.adafruit.com/adafruit-all-about-arduino-libraries-install-use. It explains how to install a library correctly.

  31. Thomas
    November 28th, 2014 at 16:57 | #31

    Hello Lauszus,

    I am trying to build an autopilot for my small boat, based on an Arduino and the Pololu MinIMU-9 V3. Your article is really helpful to me and I have now successfully (for the most part) implemented your Kalman filter in my project, using the compass (magnetometer) and one gyro as inputs. I have one problem:

    When I turn through heading North, the heading changes from 359 to 000. Obviously the Kalman filter needs a little time to stabilize again after this, which would be a problem if I would use my autopilot to steer heading North (000). Simply adding or substracting 360 degrees confuses the filter. Do you have any suggestion how to cope with this problem?

    Many thanks in advance for your reply!


  32. Thomas
    December 4th, 2014 at 22:15 | #33

    That is helpful!

    Thanks Lauszus!

  33. Michael
    January 6th, 2015 at 04:55 | #34

    Thank you, Sir! Tomorrow Iยดm going to play with this code.



  34. antix
    January 12th, 2015 at 06:28 | #35

    How to filter the acceleration components , accX, accY and accZ using kalman filter and what should be modified in your code to achieve them.

    Thank you

  35. chfakht
    January 16th, 2015 at 03:13 | #36

    Thanks a lot
    please can you help me to implement the kalman filter with a gyroscope and an accelerometre in SIMULINK i need it to determine indoor localisation
    plz help if it’s possible email me thanks

  36. siyum22
    February 11th, 2015 at 06:25 | #37

    @Lauszus : I am implementing this in C#. Can you confirm whether the mistake pointed out by dsl is corrected. the following …

    Hi, I think there?s a small bug in your implementation of step 7. On lines 75 and 76 you refer to the priori error covariance matrix (computer in lines 73 and 74) — but elements P[0][0] and P[0][1] have already been updated to posteriori state. Could you double check? I could be reading this wrong.

    Otherwise, this is super helpful. Great read, thanks.

    thanks in advance

  37. February 12th, 2015 at 00:33 | #38

    You could try to use a low pass filter in code.

    Sorry I don’t have time for that.

    I’m not sure if you are referring to the following bug: https://github.com/TKJElectronics/KalmanFilter/commit/5b6f40bb143240da7f134f41b79606c383635007? But I fixed it a while ago on Github. The newest version is available here: https://github.com/TKJElectronics/KalmanFilter/blob/master/Kalman.h.

  38. Dio
    February 12th, 2015 at 06:27 | #39


    I’m currently working on Kalman. I have to say you are the best guy whom I know that knows what this filter is and how it is implemented!

    Extremely clear and fantastic explanation

    Keep on bro…

  39. FriendlyFool
    February 13th, 2015 at 19:45 | #40

    What’s the link between the real x (without hat) and the estimated state x (with hat)?
    If our estimation is perfect, are they the same? The first couple of equations with the real state, we don’t use in the code?

    Thank you

  40. chfakht
    February 14th, 2015 at 01:21 | #41

    OK , but can you tell me HOW to do it ( if i need a s-function or just by using the support package in simulink)
    i’m a newbie in the field just tell me where can i find such ressources on this because i do’nt know where to look it will be very helpfull

  41. February 15th, 2015 at 20:22 | #42

    Thanks ๐Ÿ™‚

    Yes it the estimation was the same, then it would be the same. What equation are you talking about? Btw you can copy the Latex output by selecting the image and then use the following syntax: http://en.support.wordpress.com/latex/ to put it in the comments.

    I haven’t use Simulink that much and haven’t got time to look into it, sorry.

  42. February 28th, 2015 at 01:02 | #43

    Hello ,
    I have your robot balanduino and i found it really amaizing , i use it like a demostration to some child electronic’s club !! But actually i have encoutred two problems :
    1-when i compile your last balanduino code , even if i add Kalman.h file it always give me this error:

    WARNING: library USBHost claims to run on [sam] architecture(s) and may be incompatible with your current board which runs on [avr] architecture(s).

    Balanduino.ino:47:153: fatal error: Kalman.h: No such file or directory
    compilation terminated.
    Erreur lors de la compilation.

    2- The buzzer make a really high unconfortable sound , is it possible to make not so unconfortable

    This will help me a lot ! thank’s !!!

  43. February 28th, 2015 at 01:16 | #44

    Hello Lauszus
    compliments for your great contribution, I?ve applied the kalman library in my sketch and it works perfectly giving stable gyro rotations.

    The Kalman library works comparing two measures of the same angle, now the question is: can I apply the method to other measures (eg acceleration or distance) that are ?noise? affected but not have a second value as reference? Can you drive me to some example?

    Thanks a lot

  44. March 5th, 2015 at 21:31 | #45

    It looks like you have not selected the Balanduino in the boards menu. Please see the following guide: http://balanduino.net/get-started.

    Unfortunately you can’t play a different tone, as the frequency is fixed, but you can remove the jumper if it gets too annoying. Just remember to put it back on, so you don’t run the battery low by accident.

    Yes that is all possible, but you will need to make another implementation, as you change the state vector. Please read this post carefully and try to understand it. Then you should be able to come up with your own code.

  45. Per Aasland
    March 10th, 2015 at 13:21 | #46

    Is it correct now?

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

    The text does not match the code.
    From the formula, I get:

    angle = angle + dt * (rate - bias)

    which you would get it the order was:

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

    (Thanks for a great page by the way)

  46. March 15th, 2015 at 00:19 | #47

    @Per Aasland
    This is implemented correctly in the code: https://github.com/TKJElectronics/KalmanFilter/blob/master/Kalman.h#L47-L48, but you are right that the order of the two lines needed to be reversed in the blog post. It is updated now.

    Thank you.

  47. April 23rd, 2015 at 22:26 | #48

    Great tutorial! Quick question about your H matrix. If my understanding is correct, the H matrix should map the state to the sensor value space. So in this case, it should map the angle and bias to the acceleration (because we are measuring with accelerometers) space. Zk is therefore the angular acceleration and H*Xk should be the predicted angular acceleration. This way, the innovation, Yk is measured in the same space. The problem I am having is that if H = [1 0] , and Xk = [ [theta], [bias] ], then you are not mapping to the acceleration space, but simply comparing the measured acceleration with the predicted angle. This does not seem correct to me. If you could explain your reasoning for H, that would be awesome.


  48. Roger
    April 27th, 2015 at 13:45 | #49

    Goodday Lauszaus,

    Great article thanks. I’ve implemented the algorithm but are not convinced with the estimated position I am getting from my accl/gyro IMU. The position estimate from the Kalman does converged after a while but seems to exactly follow the position obtained from the accelerometer, which is noisy. Any ideas? Thanks.

  49. Roger
    April 27th, 2015 at 15:41 | #50

    Actually, I found the problem. I’m implementing the Kalman filter algorithm in Simulink using a Matlab function. Its, a bit different from your c++ source code but the concept is the same. I found that I have not updated the error covariance matrix P for the next iteration. After the correction and reading IMU values, the filter really works well. Your documentation was of great help, thanks again.

Comment pages
1 3 4 5 6 7 2868
  1. No trackbacks yet.