I used to have a tuning rig for my quadrocopter but I forgot it at my parents place before I stopped working on it for more than a year and now the rig is gone. Now that I want to continue working on the quad I had to build a new tuning rig.

It is a little larger and better looking than the last one and there is less friction when rotating the quad. This hopefully leads to a better tuned software. I managed to tune the angular rate mode this weekend, I think it will be flyable right now but I haven’t tested it yet due to bad weather. I’ll upload a video as soon as I have tried it outdoors.

This is what I have left to do with controller software:

• Filter yaw gyro signal, it’s to noisy right now to use as a feedback signal for yaw control
• Activate and tune feedback control for yaw
• Tune pitch/roll control in angle mode
• Activate battery voltage measurement, blink LEDs with different speed depending on battery voltage. (and install LEDs on the quad)
I also started to work on a v. 2.0 of the controller with a faster processor, more sensors, more outputs and more inputs.

I started talking about multirotors with a colleague a couple of days ago and I suddenly got the motivation back to finish my quadrocopter that I started to build over a year ago. I wanted to build my own quadrocopter controller based on my kalman filter for angle estimation. But I never really got the control parameters tuned good enough for stable flight.

I don’t think I’ve posted any pictures on the quadrocopter in my previous posts so I’ll post some here.

and some videos to show how far I got with the controller tuning.

The first thing I did when starting to work on the controller again was to rewrite the controller part of the code from scratch. This time I think I’ll implement two different control modes, one that I call stable and one that I call acrobatic. Actually I’m more interested in the stable mode since I want to use the quad as a camera platform but it’ll be nice to have an acrobatic mode as well. This is two rough sketches on how I plan to implement them.

In the stable controller the angle of the platform is controlled by the radio, while the acrobatic takes the angular rate as an input. If you tilt the stable platform 15° and then release the controller it will return to horizontal level while the acrobatic will continue to lean 15° until you pull the lever in the opposite direction and tilt the platform back.

As I wrote in my post about the Kalman filter it is actually two identical filters for pitch and roll working completely individually. This works great for angles up to somewhere around 45° but the filter will go crazy if you start doing loops  and rolls. That’s why the Kalman filter is removed in the acrobatic mode leaving a controller that works directly on the gyro signals.

Right now the code is ready and parameters for the new PID controllers needs to be found. Last time I did this I built a rig but I left that at my parents house a while ago.

I dare not to test the new parameters without having the quadrocopter securely fastened to something heavy. On full throttle the motors produce around 1,2-1,4 kW of power which can do some real damage. I realized that the hard way…

I plan to visit my parents in a couple of weeks, I’ll post an update once i fetched the test rig.

## 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