The quadcopter : how to compute the pitch, roll and yaw

 The stick IMU from sparkfun
The stick IMU from sparkfun: ADXL345 accelerometer, ITG3200 gyroscope, HMC5883L compass.

After having introduced here the basics of an aircraft orientation and how to control it, this article is about actually computing the orientation of the quadcopter in space with sensors and with the Arduino.

 Which sensors to use?

3 axis accelerometer + compass for yaw

To get easily the orientation of a non-moving object (pitch and roll), a 3-axis accelerometer (how does it work?) can be used. For a static object, it gives the value of the gravity field on 3 axes, therefore its direction. And since it always points to the center of the earth, we can therefore know how the accelerometer is inclined with the help of man's best friend, trigonometry.

This method has been used in a lot of smartphones and gives pretty accurate results, if you are not moving. Indeed, if you start translating the phone in space, you are creating a force on it, therefore you are changing its acceleration. The assumption we made previously to compute the  orientation is not valid anymore, so the calculated orientation won't be accurate.

Another problem of accelerometers is their sensitivity to vibrations.(it is actually not a "problem"  per say, it is just a consequence of the forces created by vibrations, which are essentially shocks). If you want a stable quadcopter, you absolutely need a smooth angle! I think it is by far the most important thing about a quadcopter. If you're stuck with your PID tuning and that all the settings you try are not efficient, it probably means that your angles are not smooth/ accurate enough.

As you can see, using only an accelerometer is not a valid option. You can of course try to filter the signal and reduce the amount of vibrations received by the sensor( using some foam or anything to free the accelerometer from the motors vibrations can really change a lot the output of the sensor) but it will never be precise/ fast enough to satisfy the needs of this application.

Gyroscope

The most common used sensor in quadcopter control boards is the gyroscope sensor. It  gives the angular rate around the 3 axes of space in deg/s, so, as for the accelerometer, some simple maths are needed to compute the actual angle by integration. But using only a gyroscope raises problems:

  • The first problem raised is caused by the nature of the sensor. It just gives an angular rate, not an absolute measure. So if you start up the quadcopter on a crooked floor, the initials pitch/ roll angles shouldn't be 0, but they will be in your program since the gyroscope will just output null angular rates!
  • All the common MEMS gyroscopes used with Arduinos have a drift. It means that even if you stay steady and don't move, the sensor will output values different than zero. The drift can be pretty big for some sensors (it can go up 2 deg/s on the z axis of our L3G4200d !), therefore ruining the accuracy of the measurement when you integrate the values ! But it's not as bad as it could sound, most of the gyro drift can be subtracted from the measurement since it's a constant value (given a certain temperature).

So, both accelerometers and gyros are bad? The answer is no!

There is actually many different ways of getting the most accurate orientation from a combination of sensors and they all have their good and bad sides. The simplest  approach we've taken so far for our quadcopter, and which turns out doing pretty good, is a complementary filter.

The idea of the complementary filter is the following : the filtered accelerometer value of the angle  is not subject to drift, so we use it to "correct" the value given by the gyroscope(more precise and less subject to vibrations noises). How do we do that? We combine the two values like so:

 Angle_{accurate} = (GyroPercentage) * Angle_{Gyro} +(1-GyroPercentage)* Angle_{Accel}

GyroPercentage is just a floating value between 0 and 1. It is typically ranged from 0.9 to almost 1, depending on how much you can trust your gyroscope and accelerometer.

The angle given by this very simple method is actually pretty accurate and isn't too much time consuming for the Arduino. So if you don't want to go too deep into  the maths, I would suggest you to use this method, it is very satisfactory and easy to understand 😉

On the yaw axis, the same complementary filter is used  with a compass (which gives the direction of the earth's magnetic field), giving us the 3D orientation of the quadcopter like so:

 Yaw_{accurate} = (GyroPercentage) * Yaw_{Gyro} +(1-GyroPercentage)* Yaw_{Compass}

Last, but not least, filter your signals!!  It is fundamental to reduce the impact of the frame vibrations on the angle computation, if your computed angles are not accurate, your quadcopter will never fly. It is really the most important thing to do. And filtering the signals means both in software and in the conception of the frame. Use strong materials (such as carbon fiber), try to protect your IMU( Inertial Measurement Unit = Accelerometer + gyroscope + compass) from the vibrations. On the software side, if your sensors have a built in low pass filter (like on the ITG3200 and the L3G4200D ), activate them by writing in the good registers.  If you want to program  your own software low pass filter for you sensors, a simple method is to smooth the values given by the sensor like it is done in this pseudo  C code:

/* This code shows an easy way to smooth readings from a sensor subject to
 high frequency noise.
It uses a low pass filter on a circular buffer.
This circular buffer always contains the last BUFFER_SIZE-1 readings from
the sensor.
The new reading is then added to this buffer, from which wecompute the
mean value by simply dividing the sum of the readings in the buffer by the
number of readings in the buffer.
*/

int indexBuffer;
float circularBuffer[BUFFER_SIZE];
float sensorDataCircularSum;
int BUFFER_SIZE; // Number of samples you want to filter on.
float filteredOutput;
float sensorRawData; // typically the value you read from your sensor
 //in your loop() function

void smoothSensorReadings(){
 // We remove the oldest value from the buffer
 sensorDataCircularSum= sensorDataCircularSum - circularBuffer[indexBuffer];
  // The new input from the sensor is placed in the buffer
 circularBuffer[indexBuffer]=sensorRawData;
// It is also added to the total sum of the last  BUFFER_SIZE readings
// This method avoids to sum all the elements every time this function is called.
 sensorDataCircularSum+=sensorRawData;
// We increment the cursor
 indexBuffer++;

 if (indexBuffer>=BUFFER_SIZE) indexBuffer=0;// We test if we arrived to the end
//of the buffer, in which case we start again from index 0
 filteredOutput =(SensorDataCircularSum/BUFFER_SIZE); // The output is the the mean
//value of the circular buffer.
}

What is basically done here is just an average value of the BUFFER_SIZE last inputs from the sensor. This is a simple way to get rid of high frequency noise and can also be used to smooth PWM inputs from a RC control, for example 🙂

If you made it until here, I hope you are now able to implement by yourself your own implementation of orientation computing. It's actually a big area of research and much more complex solutions are still developed ( I invite you to look at Sebastian Madgwick  fusion filter algorithm 🙂 ).