## Thursday, September 4, 2014

### Fast Quaternion Integration for Attitude Estimation

One of the central challenges in creating a flying machine control system is attitude estimation.  The better the estimate, more correctly the control system can make flight adjustments.  The better theses adjustments are, the better the aircraft can go where it's supposed to go or stay where it is wanted.
I will be writing a series of blog posts that go into detail describing how I have chosen to design my IMU complementary filter for attitude estimation.  Each post will introduce a new sensor and explain how I use it to enhance the output of my system.  To begin with I will start with the gyroscope.

Gyroscope:
This sensor outputs the measured rotation rate around three orthogonal axii.  The integer output of the sensor can be converted to degrees per second or radians per second.  For programming/mathematical reasons, radians per second are almost always used.  The gyroscope is the most important sensor relating to the stability of the aircraft, it is the only sensor required for a manually piloted quadcopter.
The main strength of the gyroscope (gyro for short), is that it has a high resistance to noise/vibration and it provides very quick response to changes in aircraft movement.  This allows the control system to respond to rotational deviations rapidly, which is the foundation of the quadcopter control stability.
This ability to respond quickly and resist noise also makes the gyro the foundation for the attitude estimate system.  In addition to stability control (where un-intended rotations are resisted), it may also be desirable to determine how the aircraft is oriented in space (pitch, roll, heading).  By taking the readings from the gyro and multiplying them by the time since the last measurement, the reading can be converted from radians per second to radians.  With some fancy math, we can take each of these little slices of rotation and combine them in such a way that we can track the total amount of angular movement we have made.  It is this math algorithm that is the heart of any attitude estimate system.
The biggest weakness of the gyroscope is that over time error can accumulate in the attitude estimate.  So for short periods of time is can be very accurate, but over "very long" periods of time an observer will tend to see something generally called gyro drift where the accumulated error becomes perceptible and slowly grows larger.  It is common to use other sensors to help counter this long term accumulation of error.  Future posts will go into detail on how I accomplish this.

In order to take the gyro readings and combine them in a way that allows for tracking the orientation of the aircraft, some type of mathematical tool must be used.  This tool should have the following abilities or characteristics:

1. Be able to combine a very small change in orientation with the previous attitude estimate so as to create a new attitude estimate.
2. Be able to rotate a 3D vector from the aircraft reference frame to the world reference frame.
3. Be able to rotate a 3D vector from the world reference frame to the aircraft reference frame.
4. It must be immune to the phenomenon known as gimbal lock.

There are two mathematical tools commonly used that meet these requirements.  These are a rotation matrix and a quaternion.  The rotation matrix consists of a 3x3 grid of numbers while the quaternion is composed of one real number and three imaginary numbers.  I'm not going to go into extreme detail on how exactly these things works, as that's beyond the scope of this article, but I will talk about the characteristics that make them more or less attractive for my use.  Both of these tools use a form of multiplication to combine rotations.  So the multiplication of rotation 1 with rotation 2 will result in a rotation which describes a single rotation that includes both 1 and 2.  As is the case with all 3D rotations, the order of multiplication effects the result (A*B != B*A).
When I started this project I was working with the Arduino platform with the Atmel Atmega328P micro-controller.  This is a very easy unit to work with given the Arduino platform, but is not very powerful.  As you can read about in my early blog posts, I intended to use matrix math to perform my attitude estimate tracking.  The draw backs to this "tool" are the following:

1. Matrix multiplication requires a large number of floating point multiplications, which is time consuming for a low powered processor (27 mult, 18 add/sub).
2. The matrix is really a set of orthogonal 3D vectors.  Over time these vectors become non-orthogonal, which reduces accuracy and perpetuates more error.  Solving this requires periodic corrections to the orthogonality, these are a waste of processing power and time.
3. As noted in point 2, the matrix is composed of vectors.  For the matrix to function correctly these must be unit vectors.  Over time magnitude error accumulates due to the lack of precision in the 32bit float and these vectors change in magnitude.  These three vectors should be re-normalized periodically, which again is a waste of processing power and time.
After struggling for a while with trying to handle the above issues elegantly and efficiently on a low power processor, I decided to put some effort into learning to use quaternions.  This was initially a bit of a challenge as quaternions are significantly less intuitive to understand and manipulate than a matrix.  Once I got a grasp of the basics of how to use them, I really liked what I saw.  The following advantages stood out to me:
1. There are no issues with orthogonality error.
2. If a quaternion does need to be normalized, it would only require one normalization operation (unlike the three required for a rotation matrix).
3. Multiplying two unit quaternions to combine rotations required fewer operations (16 mult, 12 add/sub)
4. Storage requirements are lower (4x 32bit floats compared to 9x).
Once I decided to use quaternions for attitude estimation, I needed to figure out how to convert the rotation rate vector generated by the gyro into a unit quaternion that would represent the very small angular change since my last attitude update.  The most obvious option to me was to convert the gyro vector into angle-axis representation and then convert that into a quaternion.  This was perfectly functional, but it was time consuming for the micro controller to create this incremental update quaternion due to "expensive"  operations such as trig functions, divisions, and square root.  I decided to apply the small angle approximation to this equations to reduce the processing time required to create it.  The results were amazing.  The set of equations simplified to a much greater extent than I had expected.  It went from requiring expensive sin/cos functions, square root function, and division to only needing 8x multiplications and 3x add/sub.  Here is a link to my derivation and analysis of this simplified equation.  For reference, here's the equations:
t_2 = timeDelta * 0.5
Q.x = gyro.x * t_2
Q.y = gyro.y * t_2
Q.z = gyro.z * t_2
Q.w = 1.0 - 0.5 * (Q.x * Q.x + Q.y * Q.y + Q.z * Q.z)
The quaternion Q generated by this set of equations can then be multiplied by the previous attitude estimate to create the current attitude estimate:
Q_orientation_current = Q_gyro * Q_orientation_last
Since the gyro values are generated in relation to the aircraft's frame of reference, the order of multiplication is as above.  If we were able to observe and measure the aircraft's rotation rates in relation to the earth's frame of reference, then the order would be reversed.
On an Atmel Atmega  328P (16Mhz) creating the incremental quaternion and multiplying it by the previous attitude quaternion requires 370us.  This is significantly faster than can be done by rotation matrix (588us) allowing this very low power processor to process gyro samples at a rate that can significantly reduce the error caused by rotational non-linearity (acceleration) or the saved time can be used to perform other operations.  As was noted earlier, this solution requires no wasted time correcting orthogonality errors and only requires normalization extremely infrequently.

The next two posts will cover how my quaternion based complementary filter can use accelerometer and magnetometer data to compensate for gyro drift.

Please let me know if you have any questions.

Phillip

## Wednesday, January 22, 2014

### Multi-Copter I/O Shield v2

Hello all,

Today I will talking about my experience with my multi-copter i/o shield and where I'm going with it.  Over a year ago I built my first MIOS (Link).  This post is one of the most popular on my blog.

 The original MIOS

It worked great as an interface for the working with the data buses on the Arduino Due, but resistor based voltage divider for dealing with the RC input was really sub-optimal.  It also only used 3.3v pulses to control the ESC/servos, which again is not the greatest.
Having used version 1 for over an year now, I have decided to make a second version.  This version MIOSv2, brings a whole new level of flexibility and usefulness to the Due platform.

I have replaced the resistor voltage divider with "real" logic level voltage translator.  This unit is the TI TSX0108E signal translator.  I have used two of these units on the new board.  This gives me 16 channels of 5v I/O (14x or on the right angle header and 2x are next to the lower translator ship).  And since the chips are automatically bidirectional, this signal voltage translation is invisible.  Yes, this means that if you decide to, you could attach 16 servos by only using the normal Arduino configuration and it will work without any additional consideration.  If you want to alternate input and output every other pin, you can do that.  If all you want are inputs, again, it's as easy as writing your program to use the associated pins as inputs.

 New MIOSv2 before adding the components.

These 5v I/O are connected to Arduino pins 24 through 39.  This mean that none of the special function pins are obstructed by this functionality.  All of the PWM, DAC, ADC, CAN, I2C, SPI, and Serial pins are completely available to use even if you employ every single 5v I/O.

 Complete MIOSv2 attached to an Arduino Due

Like my previous version, I have grouped the major I/O buses with ground and 3.3v power.  This gives you three SPI, 2x of each of the two I2C (aka TWI, or Wire), and both Serial 2 and Serial 3.  I don't break out Serial 1 as it is used for communication to the Aduino software (but if I needed it it would be trivial to grab it from the perimeter and get the ground and 3.3v from any other group).  All of the I/O pins around the perimeter are brought to the inside with an 0.1" spacing to make it easy to solder wires/headers to use them.
I have also added solder points for bringing power to the shield to power the Due independent of the power jack or USB port.  This power input is run through a PI filter to reduce the electrical noise from adjacent motors, ESC, servos, radios etc...

I have made the decision to sell these as there has been an large degree interest in the original version.  I did not want to sell that version as I didn't feel it was really as good as it should be.  This new version fixes all of the reservations I had with the original and works perfectly.  The initial batch is in manufacturing and testing.     I have started to develop a store front for my long term sales, but in the short term I will probably sell the initial batch on eBay or directly to get these out in the field while my store is being developed.  If you are interested please send an email to PhilsTechCorner@gmail.com.  I will be selling this initial batch for \$20 each (Future production runs will be \$30 each).  These are 100% guaranteed to function perfectly, the only difference in this initial batch is that all of the SMD components are hand soldered and some of the printing on the boards is a little lighter than I would have preferred.  Orders are first-come-first serve and should ship out by last week of January.  Arduino Due sold separately.

That is all for now.  Let me know if you have any questions.

~Phillip

## Monday, December 30, 2013

Hello All,

I apologize for my lack of posts in the last year.  My multicopter projects have taken a back seat to some other interests and to "real life" priorities.  I anticipate putting more time into posting about what I am doing over the winter.  As part of that, I have started uploading some of my Arduino libraries to my Github account.  These are libraries that I have personally written for my quad copter project.  I have a couple of disclaimers in relation to these libraries:

1. I am an amateur coder.  These are written to accomplish things the way I think they should be done.  There are probably better ways to do everything I do.
2. These are written for the Arduino platform and will work for both 8-bit Atmel processors and the 32-bit SAM.  Although the code is basic C++, I will not be adjusting them to work outside of the Arduino platform.
3. I would love feedback from anyone who finds these libraries useful.  Having said that, I generally will not be adding additional functionality to them unless it benefits me.  I know that sounds selfish, but I only have a limited amount of time and coding for others when I could be working on my own projects just isn't in the cards at this time.
4. I am working on the documentation for these libraries, but it's probably never going to be perfect.  If experimenting and reading the code in my libraries to figure out how things work doesn't sound like fun to you, then these may not be the right libraries for you.
5. These are a work in progress.
Ok, now that that's out of the way, here's the fun stuff.  I have uploaded the first two libraries and they are used for sensor filtering.

Median Filter:
This is an one dimensional filter that receives data as INT data type and sorts it from highest to lowest value and returns the median result.  This type of filter can be very good at rejecting "spikey" noise while not introducing a large filter lag.  I use this on my accelerometer readings due to the level of noise I see from motor and propeller vibration.

Usage:

#include

MedianFilter filterObject(window size, filter seed);  // Object creation.
...
filteredData = filterObject.in(rawData); // use the "in" method when adding data to the object, it returns the latest result.
...
filteredData = filterObject.out(); // use the "out" method to retrieve the current result without submitting new data.

Average processing time (micro seconds) when submitting new data (given certain window sizes):
 3 7 15 31 63 127 255 Due 2.11 3.27 5.39 9.56 17.8 34.3 67.5 328-16 18 29.4 50.5 92.3 176 340 677

Notes:
Avoid using a lot of these with large windows sizes.  Although I have tried to keep the memory needs as small as possible, the amount of memory needed to store a lot of numbers adds up quickly.

Low Pass Filter
This is an one dimensional, exponential decay, low pass filter.  It accepts data type INT.  I prefer this filter over a sliding window filter for a couple of reasons.  The first is that is is very fast, even on the 8-bit processors.  The second reason is that new data has the highest effect immediately after it is added to the filter, and has diminishing effect as new samples are added.  This contrasts to a sliding window where the data has constant effect as long as it exists in the window.  Both filter types have equivalent long term effects.

Usage:

#include

LowPassFilter filterObject(filtering power, filter seed); // Object creation.

...
filteredData = filterObject.in(rawData); // use the "in" method when adding data to the object, it returns the latest result.
...
filteredData = filterObject.out(); // use the "out" method to retrieve the current result without submitting new data.

Processing time (micro seconds) when submitting new data (based on filtering power):
 1 2 3 4 Due 0.48 0.48 0.48 0.48 328-16 2.9 3.2 3.5 3.8

Notes:
Each increase in filtering power doubles the effect of this filter.  Both the "lag" and the "smoothing" effect are doubled.  A filter power of zero has no effect on the data.

I have designed these two filters so that I can "stack" them as follows:
filteredData = LPF_object.in(MF_object.in(rawData));
When I get around to uploading my library for the Invesense MPU-6050, it will be more apparent why I have done this.  In addition, you will notice that both of these filters use data type int, this is intentional as it is much faster this way.  My final control/orientation computations are done as floats/doubles but avoiding converting to them until as late as possible helps keep my control loops from getting too slow.

Over the next month I intend to release the following libraries:
MPU6050 - my library for the Invesense MPU-6050 (nearly finished, re-verifying it's function)
Math3D - my Quaternion and 3D vector library (re-writing to include operator overloading)

My next post, I will be discussing my experience with my Multicopter I/O Shield (MIOS) and my future plan for it.

That is all for now.  Let me know if you have any questions.