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.

## Upgrading Turnigy 9x with er9x firmware

A couple of weeks ago I ordered a Turnigy 9x RC radio transmitter. This is a cheap but capable transmitter sold by HobbyKing. I’m very impressed by the quality of the hardware and the thing have more buttons, switches and channels than I’ll ever use, but I’m not really satisfied with the firmware.

Luckily there are a lot of custom firmwares for this radio and from what I’ve read, the one named er9x is currently the preferred one. Guides on how this update is done can be found on many places (for example here), this is how I did it:

I used a USBasp AVR programmer, these can be bought off eBay for a couple of dollars. Since i generally prefer PIC processors, this was nothing I had so I ordered one. But I guess you could use any programmer supported by AVRDUDE.

When you have your programmer, there is a software called eepe that is a specially made AVRDUDE frontend for programming er9x firmware. I downloaded and installed that program.

Then you need to connect your programmer to the processor, this is the hardest part, but not really that hard. First unscrew the six screws on the back of the radio. Then disconnect the cable hold the two halves of the radio together.

after that you need to locate the connection points where the programmer should be connected. These are all around the processor in the lower part of the radio. NOTE: I have the v2 version of this radio, the connection points differ on the older v1 of this radio!

To connect the USBasp programmer I made a custom cable like this

Note that the leads 3, 6, 8 & 10 are cut off and not used.

I soldered the cable to the connection points and followed the instructions for eepe. When I was done, I left the soldered cable inside the radio if I want to flash it again someday.

## Not much happening right now

It’s been a while since I had any updates here. I’ve had a lot to do and, no energy to work on any of the projects here for a while. I think that the first project I’ll resume when the summer is over is my Prusa Mendel RepRap.

Whit this done I could start making parts for really cool multicopters.

## Propeller driven Lego car.

Lego Technic is perfect for creating quick prototypes of pretty much anything, combine this with RC stuff and you can create really fun stuff. One simple example is the robot i describe in this post:
Sunday afternoon robot

My old Lego is usually stuffed away in a box in the basement, but when we have kids visiting we usually bring it up to the apartment. I suspect I’m the one enjoying it most and this time I built a RC lego car

After a few testruns I added a propeller protection bar to minimize the risk of running into something with the propeller.

Runs great, I’ll probably update this post with a video when I’ve recorded one.

## Plans for my depron plane

In a comment on the post about my first Depron RC-plane I was asked for the plans. I’m afraid I don’t have the nice 1:1 print on multiple A4 plans that most people are used to, but if someone have a good guide on how I create those kinds of prints I’ll be happy to create one. But for now I can provide the drawing with measurements.

And the same drawing as a PDF

I measured up these parts on a piece of cardboard to use as a template. Especially since you need two of all parts except the one in the lower right.

I used the kind of cardboard that you can buy at a painters shop to protect your floor while painting.

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

## Sunday afternoon robot

I had nothing to do today so I copied hubbens creation from the Swedish electronics forum. It is a two wheeled radio controled vehicle based on just two servos.

For wheels I used two wheels from an old Lego Technics truck I got for christmas almost 20 years ago (The best christmas gift I’ve ever got BTW). The wheels were bolted to servo horns from two HXT900 servos.

These servos are modified for continous rotation according to this guide.

Pretty simple:

1. Open servo and remove the gears and potentiometer.
2. Connect to your receiver and set your output to center. (or use another way to generate a 1.5 ms pulse every 20 ms)
3. Adjust the potentiometer to the center until the motor stops and solder it so it can’t rotate any more.
4. Remove the stop pins on the output gear with a sharp knife
5. Sand the top of the potentiometer axis so the top gear spin easy on the axis.
6. Re-assemble the servo

The servos are then glued back to back and some self adhesive velcro are mounted on both sides and the wheels are mounted.

The ideal would be to run both the receiver and the servos of a 2S LiPo but I only have 3S at home so I needed to use a Turnigy 8 V – 10 V 5A SBEC to power the servos and the receiver. Together with the DC/DC converter the über cheap Hobby King 2.4Ghz 6Ch receiver was mounted on the top of the servos.

On the bottom I have a Turnigy 1000mAh 3S 20C Lipo which is a little to large for this project but it was the smallest one I had.

I used channel 1 and 2 and mixed them 100% with each other. Since the servos are mirrored they need to rotate in the same direction for a turn and in oposite directions to move forwards och backwards.

This is what it looked like when it was ready.

My next order from HobbyKing will include a smaller 2S battery. And maybe I’ll put together a small controller board for this to make it autonomous.

Since my first control theory class at the university I’ve been thinking about building a quadrocopter. There are several “off the shelf” kits you can buy that handles stabilization of the quad, KKMulticopter being the most popular one. As usual I did not want to use anything someone else made so I started building my own controller. This post will focus on the electronics of this controller but there will be more posts later on about the controller software and the mechanic construction of the quad.

Starting by defining the requirements on the controller

• 4 PWM outputs to the ESC
• Power supply from one ESC
• Processor capable of handling floating point Kalman filter
• 3-axis Accelerometer & Gyro sensors (digital)
• Capable of flashing several LED
• Capable of measuring battery voltage

The ESC (Electronic Speed Controller) is a controller for running 3-phase BLDC (BrushLess Direct Current) motors. It takes a standard RC radio PWM pulse as input and outputs a synchronous 3-phase trapezoidal voltage to the motor. Most ESC of the size used in this project includes a 5 V 2-3 A linear voltage regulator to power the receiver from the main battery. This power will be used to power the controller as well through a 3.3 V LDO regulator.

The three axis accelerometer and two axis of the gyro are needed to estimate the pitch and roll of the quadrocopter using a Kalman filter. The third axis of the gyro is used to implement a simple heading hold control loop.

Let’s just say that I’m more of a software, than electronics, guy. Electronics is really fun and interesting but I’m no professional. If someone sees any errors or strange things I’ve done in this design, please tell me and let me learn from this.

If i start with the processor, I have previously used the Microchip dsPIC line of processors and like them very much. They are easy to work with and the student version of the C30 compiler works fine for my needs. I had some dsPIC33FJ32MC204 laying around at home from an old projects and these have a motor control PWM module with 4 outputs and 4 input capture modules capable of reading the signal from the RC receiver. A simulation of the Kalman filter code I’ve written told me that performance wasn’t an issue either.

I used the Free Version of Eagle Layout Editor to draw up a schematic

And create a PCB layout from this schematic. The layout was adapted to a 50 mm x 50 mm pcb since I found the extremely cheap PCB manufacturer ITead Studio which could make 50 mm x 50 mm PCBs for less than \$10. The layout used two layers and looked like this.

The sensors are from Sparkfun and mounted on breakout boards, since this is my first PCB order I played it safe and didn’t want to solder the extremely small packages of the sensors.

ITead Studios is based somewhere in China and it took several weeks to get the PCBs but when they arrived the all seemed to be of good quality

I have a couple of these boards left, contact me if you want one.

I used a toothpick to apply solder paste from dealextreme to the PCB, placed the components and heated with an ordinary soldering iron. Quite time consuming but I’m pretty sure that the end result was better than if I would have used ordinary solder. Not the prettiest soldering job but it was my first using this technique.

Now the “only” thing left to do is build the mechanical part of the quadrocopter and create the software. But I save that for another post…

## First try on depron RC aircrafts

This will be my first project post on this blog starting with something I made a year ago. A RC aircraft made out of depron.

Depron is a foam material intended for insulation and sound dampening of floors. I bought a 10-pack of 6 mm and a 10-pack of 3 mm at Hornbach, a local hardware store, which will be enough for several planes. At this store the foam was actually called Ebisol instead of Depron, but I guess those are just two different brands of the same stuff.

There are thousands of freely available plans for such a model only a google search away but I had to design my own. Some time behind a CAD program I ended up with this.

If I were to build this model again i would definitively make some changes to the plans. Some parts of the model were very hard to assemble due to their complex shapes.

A couple of evenings of assembly and some inspections from the chief engineer

The model were done.

Almost all parts I used for this were bought from Hobby King

Apart from this i bought some carbon rods and some piano wire from a local RC store.

The only thing left to do were the test flight

That was the end of that aircraft. I’ve heard people say that the center of gravity was off making the plane unstable. I believe Murphy was trying to teach me a lesson that I should try to walk before I run. Maybe I should have used one of the existing plans known to work with a well defined spot for center of gravity.

As I said this was a year ago. Two weeks ago I visited Aeroseum which is an aircraft museum here in Gothenburg. By now my emotional wounds, from seeing hours of work crashing to the ground, have healed and i feel ready to make a new try.

More about that in a future post…