Matlab attitude estimation

From Gluonpilot

Contents 1 Intro 2 Data 3 Algorithm 1: Complementary filter 4 Algorithm 2: Quaternion + PID 5 Algorithm 3: 2x3 Kalman

Intro

The gluonpilot has 5 on-board sensors that are used during attitude estimation:

• 3-axis accelerometer (ADXL335)
• 2-axis gyroscope (IDG-500)
• 1-axis gyroscope (ADXRS613)
• Pressure sensor (SCP1000)
• GPS

+ an optional magnetometer and pitot tube (not used currently)

There is not one straightforward way to combine these readings to a pitch, roll and yaw angle. For the gluonpilot, we need to find the most accurate algorithm with the least overhead. Looking at all the possible combinations, we need a good way to simulate all this. Test flights take too much time!

Todo: write "introduction to attitude estimation"

Data

There are 2 ways to create a simulation environment/sensor inputs:

• create a model for our UAV and our sensors
• use real-life sensor data

Because we want to build a real flying MAV and the gluonpilot can perfectly log all this data, we will use the 2nd option. Verifying the results of our calculations (pitch and roll angle) is not always easy. Usually at some point, the result of 2 algorithms can significantly differ. The flight log in Google Earth can be used to check which one is the most correct.

Overview of the pitch angle using different algorithms over the complete flight:

Path 1: Slight decent with negative pitch:

Path 2: clearly a decent with negative pitch:

Path 3: the UAV is clearly losing altitude with a negative pitch. In the middle, a slight recovery to 0° pitch is notices, followed by another slight decent: Algorithm 2 with the PID loop is clearly wrong on this 3th path.

Algorithm 1: Complementary filter

Problem: gyroscope effects are immediately averaged out & averaging causes a delay.

Algorithm 2: Quaternion + PID

This algorithm is pretty easy. These are the most important steps:

1. Add the calculated bias to the gyroscopes' readings
2. Use p, q & r from the gyro's and calculated biases to update a quaternion which holds the orientation.
3. Substract the dynamics from the accelerometers' readings (u = speed along x axis, w = speed along z axis, r = roll rotation rate, q = pitch rotation rate, r = yaw rotation rate):
   1. Gravity vector (x) = acceleration(x)
   2. Gravity vector (y) = acceleration(y) - (r * u - p * w)
   3. Gravity vector (z) = acceleration(z) - (p * w - q * u)
4. Use the calculated gravity vector to calculate pitch and roll (arc tangens of gravity vectors):
5. Calculate the error between the quaternion's pitch and roll angles and the angles from the gravity vector
6. Use these errors to update the gyro's bias with a PI(D) loop.

These steps are more or less similar to what Mahony described in a paper. I must say it works better than expected. However, there is still some inaccuracy on the pitch axis after a turn.

The result is called "pitch PID" in the graph above.

Algorithm 3: 2x3 Kalman

This algorithm was taken from a paper by Beard and adapted to the gluonpilot's specific needs.

1. The state x contains the roll and pitch angle
2. Update x with the standard earth-to-body transformations:
   1. roll = roll + (p + q*sin(roll)*tan(pitch) + r*cos(roll)*tan(pitch)) * DT
   2. pitch = pitch + (q*cos(roll)- r*sin(roll)) * DT
3. Update the P-matrix of the extended kalman filter
4. Calculate u (speed along x-axis) and w (speed along z-axis) from the GPS and pressure sensor.
5. Calculate matrix h, which contains the estimated accelerometer readings:
   1. acceleration(x) = q*w + sin(pitch)* G
   2. acceleration(y) = (r*u - p*w) - cos(pitch)*sin(roll) * G
   3. acceleration(z) = (p*w - q*u) - cos(pitch)*cos(roll) * G
6. Calculate the Jacobian dh/dx to complete the extended kalman filter.
7. Update the pitch and roll angle (state x) with the error between "h" and the actual accelerometer readings with the innovation matrix.

Despite the fact that no gyro bias is calculated, this algorithm perform surprisingly well. Actually a lot better than the previous one.

On the downside:

• Special care needs to be taken because this algorithm uses euler angles, which are not gimbal-lock free
• Requires more processing power

The result is called "pitch 3x3" in the graph above.