# Pitch and roll estimating Kalman filter for stabilizing quadrocopters

A while ago i wrote a document on how you could use a Kalman Filter to merge the sensor readings of a gyro and an accelerometer into pitch and roll for a quadrocopter. This is pretty much a copy/paste of this document with some small adaptions for the blog format. I haven’t got my quadrocopter to fly yet using this filter but I’m pretty sure that it’s possible if I just find the time to trim all paramters of the filter and controller.

The aim of this post is not to present all the equations proving the optimality of a Kalman filter, there is enough papers and books doing that. Instead you should, after reading this, understand my filter implementation enough to be able to tune it for your purposes. Because of this the document may not always be 100% mathematically correct and concepts I do not find important may be left out completely.

To understand this paper you need to know some basic trigonometry and linear algebra. For those of you not comfortable with linear algebra I suggest you to read a little. The Wikipedia articles about matrix multiplication, matrix inversion and matrix transpose could be a good start. I will not go into detail on explaining exactly how the Kalman filter works but I will explain the steps needed to construct such a filter. For a more in-depth view on the Kalman filter I recommend you to read the Wikipedia article or one of the several good books on the subject. This document will however use the same notations as in the Wikipedia article.

The reason I constructed this filter in the first place was to control the pitch and roll of a quadrocopter where neither the accelerometer nor the gyro could be used by itself to get accurate readings. The accelerometer can be used to estimate the direction of the gravity acceleration vector while stationary but this becomes difficult when the quadrocopter is moving since acceleration composants will appear in other directions than downwards. The gyro reading on the other hand can be integrated over time to get the angle relative to starting position but each measurement error will integrate into the angle estimation and the error will build up over time, especially since most gyros have a small bias. One of the solutions of this problem is to fuse the readings from both sensors using a Kalman filter. This filter uses the gyro readings for quick angular changes and the accelerometer to correct the error over longer periods of time. Other solutions like DCM or complementary filters exist but are not explained in this document.

# The filter

A composite Kalman filter for estimating both pitch and roll could be created, taking into account the fact that they are coupled through the accelerometer gravity vector. Such a filter would be very complex and highly nonlinear because of the trigonometry involved forcing us to use an Extended Kalman Filter (EKF). It will probably require a large amount of trigonometry function evaluation and will not, on a microcontroller without hardware floating-point capabilities, yield enough performance to balance a quadrocopter. Instead the filter could be divided into two identical filters one for pitch and one for roll. This not only makes the filter linear but also removes almost all computationally heavy trigonometry functions.

This simplification is based on the fact that the changes in gravity acceleration vector for pitch and roll are more or less independent of each other for small angles when the angle approaches ±90° there will be errors in the output. The filter constructed will work best for small changes of pitch or roll which generally isn’t a problem when stabilizing a quadrocopter. Hence this filter may not be optimal for acrobatic flight performance and maybe more suitable for aerial photography or similar tasks.

# Steps of evaluating the filter

The Kalman filter consists in six steps executed over and over again with a sample time  between the executions. In the notation used in this document a variable is subscripted with  it refers to the value during previous execution and if it is subscripted with  it refers to the value during the current execution. The steps are:

## 1. State prediction based on dynamic model.

One of the most important parts of the Kalman filter is the model of the system. This is the part which keeps the output sane while the sensors are feeding the filter with insane readings. For example reducing the effect of measurement noise or the acceleration vector pointing somewhere else than down due to other acceleration than gravity. The model is used to predict the next state of the model based on the current state and the control signals.

Since we separate the estimation of pitch and roll the quadrocopter state x according to one of the filters can be described by three different variables. Keep in mind that this is for pitch OR roll. Two identical filters are created.

$\boldsymbol{x} = \left[ \begin{array}{c} \theta \\ \dot{\theta} \\ \dot{\theta}_b \end{array} \right]$

The angle  the angular velocity  and the bias in the angular velocity measurements  Using this state representation the equation

$\boldsymbol{x}_k = \boldsymbol{Fx}_{(k-1)} + \boldsymbol{Bu}_k + \boldsymbol{w}_k \: \: (1)$

Can be used to make a good guess of the states in this sample  based on the state in the previous sample  and the current control signal The matrix  represents the dynamics of the system. Multiplied with the old states the result is a guess of the new states. For the quadrocopter the following F matrix is used.

$\boldsymbol{F} = \left[ \begin{array}{ccc} 1 & \Delta t & -\Delta t \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array} \right]$

If you expand the matrix equation for xk the result will be

$\begin{array}{l} \theta_k = \theta_{(k-1)} + \Delta t(\dot{\theta}_{(k-1)} – \dot{\theta}_{b_{(k-1)}} ) \\ \dot{\theta}_k = \dot{\theta}_{(k-1)} \\ \dot{\theta}_{b_k} = \dot{\theta}_{b_{(k-1)}} \end{array}$

If you think about it, this seems fairly reasonable. The angle in the next sample will be the angle in the sample before plus the unbiased angular velocity multiplied with the sampling time. The model also says that the other two states will remain the same as in the previous sample. This is a simplification saying that the angular velocity and angular velocity measuring bias will change slow enough to be barely noticeable between two sample times.

The next part of equation (1) involves the control signal input to the system. The multiplication with the B matrix can be expanded in exactly the same way as the F matrix but in the case of the quadrocopter inputs are ignored thus

$\boldsymbol{B} = 0$

Inputs could be taken into consideration but this will increase the complexity and computational requirements of the filter and the accuracy gain will probably not be significant.

Just to explain how this works (you can skip this section if you want), let’s say you also feed the filter with the input to the motors affecting the filter angle u1 and u2 and you also know that the force generated by the motor is linearly dependent of the input by the constant k. You also know the inertia Jaround the axis of the angle. You could use

$\boldsymbol{u} = \begin{bmatrix} u_1 \\ u_2 \end{bmatrix}$

$\boldsymbol{B}=\begin{bmatrix} 0 & 0 \\ \frac{k}{J} & \frac{k}{J} \\ 0 & 0 \end{bmatrix}$

resulting in a model for the estimation of the angular velocity looking like this

$\dot{\theta}_k = \dot{\theta}_{k-1} + \frac{k}{J}\left ( u_1 – u_2 \right )$

Since the model is pretty coarse there is a need for something more to take model errors into consideration. It is easy to see that the model for the 2nd and 3rd state will not be 100% accurate all the time. This is where w, which is called model noise, comes in. A zero mean Gaussian noise is added to each state to represent changes that doesn’t agree with the model. In mathematical terms

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

Q is the covariance matrix of the noise in other words how much noise there is and if the noise on one state depends on another state. For the quadrocopter the noise on each state is considered to be independent which makes Q diagonal

$\boldsymbol{Q} = \begin{bmatrix} q_1 & 0 & 0 \\ 0 & q_2 & 0 \\ 0 & 0 & q_3 \end{bmatrix}$

The elements on the diagonal represent how large model errors we expect in the different states and will be an important tuning parameter in the filter. The fact that this matrix is diagonal will allow us to do a lot of optimizations while realizing the filter in C-code later on.

## 2. State covariance matrix update based on model

Another important part of the Kalman filter is the matrix P which represents how much we trust the current values of the state variables. A small P matrix means that the filter has converged close to the real value. When the model predictions in step 1 have been done the P matrix has to be updated according to the uncertainties induced by the model noise w with the covariance Q. This is done with the equation

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

where the superscript T means that the matrix is transposed. You scale the current value of P with F2 and then add the covariance matrix of the model noise. This hopefully makes sense since you have made a guess on the current states based on a coarse model the uncertainty must increase.

## 3. Model and measurement differences

The next part of the filter is where the new measurements  come in. In this step the difference between the states calculated from the model is compared to what is measured using the equation

$\boldsymbol{y}_k = \boldsymbol{z}_k – \boldsymbol{Hx}_k + \boldsymbol{v}_k$

which introduces the H matrix. This is a matrix which maps the current state on the measurements. In the quadrocopter case we measure the angle based on the gravity vector by using the atan2(b, a) on the acceleration vectors perpendicular to the filters rotational axis and the angular rate measured by the gyro. This results in a measurement vector

$\boldsymbol{z} = \begin{bmatrix} \theta \\ \dot{\theta} \end{bmatrix}$

Since the gyro bias will be a part of the angular rate and cannot be measured we will leave it to the Kalman filter to find out. To map or state vector x onto the measurement vector z we multiply by the matrix

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

As with Q the zeros in this matrix reduces the number of multiplications needed when this is realized in C-code allowing further optimizations.

The vector v is similar to w it represents errors in the measurements which are assumed to be zero mean and Gaussian distributed as well

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

R is similar to Q the covariance matrix of the measurements. This can be directly related to the sensor quality. As with the Q matrix the sensor errors are assumed to be independent which make the R matrix diagonal as well

$\boldsymbol{R} = \begin{bmatrix} r_1 & 0 \\ 0 & r_2 \end{bmatrix}$

As you may have noticed by now, we like matrices with a lot of zeroes since this allow us to optimize the filter implementation.

## 4. Measurement covariance update

After including new data in form of measurements we need to calculate the covariance matrix of this data which is known as S and is similar in many ways to P. This is calculated with the equation

$\boldsymbol{S}_k = \boldsymbol{HP}_k \boldsymbol{H}^T + \boldsymbol{R}$

You can see that the value of S depends on the covariance of the previous model predictions transformed to the measurement vector through H plus the covariance of the sensor readings. Once again I want to point out that a large covariance matrix means that we have low confidence in the values while a small covariance would mean that the values should be fairly accurate.

## 5. Calculate Kalman gain

Now we are getting close to the part where we merge the knowledge from the model with the measurements. This is done through a matrix called the Kalman gain K This matrix will help us weigh the measurements and the model together. K is calculated by

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

What is done here may not be obvious at the first glance but makes sense if you think about it. You take the H matrix which maps the states onto the measurements and multiply it with the state covariance and the inverse of the measurement covariance. If we play with the thought that we have large confidence in our model and low confidence in our measurements the resulting K will small. On the other hand if we have low confidence in the model and high confidence in the measurements K will be large.

## 6. Improve model prediction

Now it is time to improve the model prediction by adding the difference between measurements and predictions  to the states predicted in step 1 after scaling it with the gain matrix K

$\boldsymbol{x}_k = \boldsymbol{x}_k + \boldsymbol{K}_k \boldsymbol{y}$

The result  will be the filter output at this sample time. Remember from the previous step that a larger confidence in the model than the measurements will result in a small K decreasing the importance of the measurements  and the opposite if we trust the measurements more than the model.

## 7. Update state covariance with new knowledge

Since we added data in form of measurements to the state vector we need to update the state covariance matrix P which is done by the equation

$\boldsymbol{P}_k = \left ( \boldsymbol{I} – \boldsymbol{K}_k \boldsymbol{H} \right )\boldsymbol{P}_k$

Where I is the identity matrix

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

in this case. This can be seen as scaling P down because we are a little bit more certain of the state values after adding measurement knowledge. As you can see P will be growing in step 2 after making predictions based on the model which of course makes us more uncertain of the states and then shrink again in this step after we corrected the predictions with measurements. After this step is done we can wait for the next sample and then start over at step 1.

# Summary

To sum up the filter in the quadrocopter case consists in the following calculations performed at each sample

$\boldsymbol{x}_k = \boldsymbol{Fx}_{(k-1)} + \boldsymbol{Bu}_k + \boldsymbol{w}_k$

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

$\boldsymbol{y}_k = \boldsymbol{z}_k – \boldsymbol{Hx}_k + \boldsymbol{v}_k$

$\boldsymbol{S}_k = \boldsymbol{HP}_k \boldsymbol{H}^T + \boldsymbol{R}$

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

$\boldsymbol{x}_k = \boldsymbol{x}_k + \boldsymbol{K}_k \boldsymbol{y}$

$\boldsymbol{P}_k = \left ( \boldsymbol{I} – \boldsymbol{K}_k \boldsymbol{H} \right )\boldsymbol{P}_k$

The filter is tuned by modifying the Q and R matrices

$\boldsymbol{Q} = \begin{bmatrix} q_1 & 0 & 0 \\ 0 & q_2 & 0 \\ 0 & 0 & q_3 \end{bmatrix}$

$\boldsymbol{R} = \begin{bmatrix} r_1 & 0 \\ 0 & r_2 \end{bmatrix}$

 Variable Description q1 How well do the model represent the changes of q2 How well do the model represent the changes of q3 How well do the model represent the changes of r1 How much do we trust the measurement of r2 How much do we trust the measurement of

These variables are relative meaning that increasing one of the variables yields the same result as decreasing all other variables. In the quadrocopter case some pointers could be given about the magnitude of these variables.

• r2 should be larger than r 1 since the gyro readings will not be affected by the quadrocopters acceleration. This will lead to the gyro responding quicker than the accelerometer.
• q3 should be low since the gyro bias change extremely slow.
• r1 should be larger than q1 to prevent acceleration from affecting the angle.
• The relation between q2 and r2 will control how sensitive the angle output is to noise and how quick it will respond to changes.

# Implementation

In some MATLAB inspired pseudocode you could write the filter as:

Since this filter is meant to be run on a microcontroller (a 16-bit Microchip dsPIC in my case), together with some form of control loop for feedback control of the quadrocopter pitch and roll, it is essential to keep execution time down to a minimum. C also lack native support for matrix operations. Because of this I have put both time and effort in expanding and optimizing the equations above.

I also choose to use floating point math instead of fixed point for the c-implementation. There are two reasons for this, I once heard someone say that Kalman filtering needs the dynamic range provided by floating point math. Maybe Q15/Q16 implementation would be enough for this application but the main reason of using floating point match would be me being lazy and the resulting execution time is good enough.

The resulting filter were finally implemented in a dsPIC33 processor running at 40 MIPS allowing for at least 500 Hz sampling still leaving plenty of cycles for executing control algorithms.

## Code example

The filter is divided into two different files. The first is a header file (kalman.h) which contains declarations and definitions. The second is the main file which contains all code (kalman.c).

I will also show an example on how this code can be called on from another file (main.c). Keep in mind that the direction of the axes differs from setup to setup.

## main.c

Thanks Ercan for pointing out my programming error in kalman.c

## 36 thoughts on “Pitch and roll estimating Kalman filter for stabilizing quadrocopters”

1. Ercan says:

Hi,

I was looking at your c -code ‘kalman.c’, Step 7,
the lines starting with “data->p11 = …” are changing the variable pXX
that subsequent lines are using as input.
If the formula is: P(k) = (I – KH)P(k-1),
then I think you should change the code to prevent that.
Secondly, shouldn’t the line
“data->p33 = data->p33 – data->p22*k32 – data->p13*k31;” be
“data->p33 = data->p33 – data->p23*k32 – data->p13*k31;” ?

1. LHelge says:

You are correct, I’ll fix that immediately! Maybe that’s why the filter have behaved a little strange on the quad in some situations.

2. Ryan L says:

I’m using your KF and I have the same problem I have with mine. It doesn’t do a very good job of filtering the horizontal accelerations. I still get rotations as great as 10deg from small horizontal accelerations.

3. Dave says:

Hi, can you sen me a changed code to me with Email. I got some trouble in Kalman filter, Thank you!

1. Andrew says:

Hi!

Is it finally working correctly??. In “Kalman.c” code we need to replace:

data->p33 = data->p33 – data->p22*k32 – data->p13*k31;
by
data->p33 = data->p33 – data->p23*k32 – data->p13*k31;

…as Ercan said?? Could you test how it works??
Thanks a lot!

4. Matt says:

Would like to know more about the Kalman Filter implemented here..Can you please help?

1. LHelge says:

Sure, what do you want to know?

5. Matt says:

What all things are affecting the roll angle? How can it be obtained clearly??

6. Jeremt says:

Thanks for the tutorial. I am implementing the kalman filter for pitch and roll calculation, however I want to add a camera to give me a second reading of the angle. This way I will have the camera and accelerometer stating the current state and I will use the gyro as control to predict the next state. Can you please guide me on how I should encorporate the camera please? which part of the code do I have to change. thanks

7. Ravi C says:

Thank you for your article – I have one question though which has been bugging me – (eq. 3) when you add z(k) + – h.x(k), the measurements z(k) are of the previous time step, where are x(k) is the new state in this current time step that does not include the effects of this time step – does my question make sense? Intuition tells me that the algorithm should be steps 3 through to 7 followed by steps 1 and 2 at each time step.

8. mike says:

The u1 and 2 should be squared, since the force is proportional to the square of the rotation rate. Thus the resulting torque has the same dependency. Also the length of the arms should be in there somewhere… And a – sign in B is missing.
tau = k*l*(u1^2-u2^2)
where k = force coefficient, l distance motor-cog.
Disregarding any u makes the filter belief there is no torque, and it will only “flatten” the gyro data. Instead using the toques compares the gyro values to physically relevant info… which is kind of the sense of a kalman filter!

1. Daniel Stenberg says:

Well that depends on the unit of the input signals u1 and u2. If they can be viewed as the physical actuation of the system, i.e. rolling and pitching moments [Nm], then it’s a different story. Likely, you won’t be able to measure the motor RPM, but you’ll know the PWM duty cycle they are controlled with.

But the real reason that the input signals may be left out (not saying they definetly should!) is that the propeller lifting forces vary A LOT depending on flight mode and conditions, e.g. hovering, ascending, descending, forward. It may fluctuate as much as 50%.

1. Daniel Stenberg says:

Just to add to my previous comments, since the fluctations are typically very high for the propeller lifting force, Q2 would have to be very large to account for this model insecurity. A very high Q2 would basically be saying “we do not trust the model”, so it’s more or less ignoring the input anyway.

But in principal you are correct regarding the usage of the input, it is what makes Kalman filters great!

9. Daniel says:

From what I was able to find, acc_pitch = atan2(acc_x, sqrt(acc_y^2, acc_z^2)); acc_roll = atan2(acc_y, sqrt(acc_x^2, acc_z^2));

10. Alin says:

Hi. I have a table of 20,000 values for each of the following: the gyro roll, pitch, (the yaw is 0 all the time), and x, y and z acceleration. What I need to do is to combine these values into a kalman filter and make a plot of the output. I intend to do that in mathcad. Could you please give me some advice about what to initialize every variable with? I hardly understand Kalman Filters so it would be really helpful to me. Thanks in advance.

1. LHelge says:

Do you have access to the software MATLAB? In that case I could send you a MATLAB implementation of the filter that would be easier to use for your task. I havn’t really worked with mathcad myself so I cannot really help you there.

1. Adi says:

Hi I saw your code but how about to those values of pitch an roll after.I have a PWM for a motors that the rate are 0 to 57 in 500Hz I want to stabilize a quadcopter.I need to configure Kallman filter for the values for the PWM?Which values I need to change? Thank you for your article.

1. LHelge says:

The Kalman filter is for estimating the pitch and roll angles from nosiy accelerometer and gyro measurement. For that to translet to control signals for your PWM you need some kind of controller. I would suggest you try a PID-controller since you have both the angle and its derrivative as an ouptut from the filter.

11. Scott says:

Great article. Your table at the end seems a tad unfinished though. It says “How well do the model represent the changes of…”. I am on the edge of my seat.

A question: If I implementing a Kalman filter where my x is position, velocity and acceleration (along one direction) would my Q now not be diagonal? In that case it seems like process “noise” acting on velocity would also act on position. I am having a really hard time getting my head around how to figure out Q.

Thanks,
Scott

1. LHelge says:

Yes you are correct, and the same applies to my filter. Maybe it’s a little bit sloppy of me to write that they are considered to be independent. In your example you would recieve a 3-state filter where velocity depends on acceleration, and position on velocity. You could certainly get a better performing filter if you include this in the Q-matrix, my guess is that the biggest gain would be a shorter settling time when the filter is started up. However, unless you have done an extensive work to mathematically find suitable values for the cross-covariances of your model, there will be a lot of parameters to tune.

Since I only have 3 different values != 0 I could tune them by trial-and-error. For example if you would get a lot of noise in your acceleration state from the accelerometer measurement, you could decrease the corresponding value in your Q-matrix to make your filter trust the model a little bit more and the sensor a little bit less. This approach would be very difficult if you also had cross-covariances to take into account.

Another reason I had for assuming no cross-covariance in the Q-matrix is that my filter was implemented in a very slow microprocessor using software floating-point calculations. A diagonal Q-matrix allowed me to remove several multiplications during each filter evaluation allowing a higher refresh rate.

12. Will Castillo says:

I found this very helpful. Thanks for taking the time to post this.

13. nerdOmat says:

Isn’t the Covariance Matrix P a symmetric matrix? Correct me if I’m wrong.

You could save some more floating point operations in step 2 by setting
data->p21 = b // = data->p12

14. Fabián says:

Hello, thanks for posting this, specially the part about ‘tuning’ the covariance matrices.

Why did you choose to measure only the angular velocity and not angular velocity+bias? (H matrix), that’s the measurement we get from the gyroscope. I have tried with H = [1, 0, 0; 0, 1, 0] and H = [1, 0, 0; 0, 1, 1] and the result looks similar, it is there another reason?

1. LHelge says:

No, you could go either way.

15. Ilya says:

Hi! I suppose you can decrease complexity of your implementation significantly! Magnitude of coefficients K stabilizes after first seconds. So you can skip steps 2, 3, 4, 5, 7 and use prepared gain coefficients. This steps influence to a transition process. But minimization of the transition process isn’t worth for a copter.

16. Sachin says:

hey linus,
i’m working on the kalman filter for implementing it in the AHRS system, this code will help me, thank you
And the kalman gain in your pseudocode has to be K=P*H'(transpose)*inv(s)
Meanwhile can you send me the Matlab implementation of this code, will be very helpful.

1. LHelge says:

I’m not sure I still have it, will have a look.

1. sachin says:

Hi, I tried implementing it on matlab it works fine by importing the data’s from csv file.
Now i’m trying to do it in real time, Although i have a few doubts doing it.
Could i get your mail-id so that i could clarify few of the things i wanted to.

1. LHelge says:

Sent an e-mail for you to respond to.

17. beka says:

is it ok to use your values for Q and R , or I have to tune them for my system ? and if i have to tune them , how can i do it ?

1. LHelge says:

You probably have to tune them for your system. I did it by trial and error. If you know the current filter behavior and the wanted behavior and how Q & R affects it, is’t not that difficult for such a small filter.

18. Albert says:

hi, i have developed a code for my quad with pid control…. the take off is stable but there is a drift in one direction and as far as i have read , the kalman filter helps in accounting for this … could u help me out in implementing the filter in my code ….. thnx in advance

19. Daniel S says:

The calculations for the B matrix are incorrect. The B matrix you’ve presented here is for the continous time model, not the discrete one which we are using.

To obtain the correct B/input matrix, you must integrate F*B from 0 to deltaTime (h below) (derivation based on discretization of continuous systems theory). This will yield input terms to the first state as well in the discrete time model.

B (discrete) will then be:
[h^2/2*k/J , h^2/2*k*J;
h*k/J , h*k/J;
0 , 0 ]

But overall, thanks for a great post!

1. LHelge says:

Thanks, will have a look at this when i find the time.

20. Teyo says:

Hi Linus, I am studying on tuning kalman filter parameters. I tried it on some examples. Also I want to try it on this pitch and roll estimating system, but ı have no information about this system. I see you shared c code for this system. If possible, may you share here or send me by mail the matlab code of the system ? Thanks…

This site uses Akismet to reduce spam. Learn how your comment data is processed.