Kalman filter algorithms design

[stenbergd]

Design linear Kalman filters for sensor fusion of gyroscope, magnetometer and/or accelerometer data.

Design estimators for

• roll angle: Use gyroscope and magnetometer
• pitch angle: Use gyroscope and magnetometer
• yaw angular rate: Use gyroscope and magnetometer
• vertical velocity (optional, depends on if we want to control this state or just control motor throttle directly): Use accelerometer and barometer/altimeter

Some useful links http://www.linushelgesson.se/2012/04/pitch-and-roll-estimating-kalman-filter-for-stabilizing-quadrocopters/

http://www.mouser.se/newproducts/applications.aspx?virtualdir=sensor_solutions_mems

[stenbergd]

Update: It is at this moment not clear if we should use magnetometer or accelerometer for state correction.

The accelerometer will not be ideal since the quadrotor's own acceleration will be added to that of gravitation. In this regard, the magnetometer will be better. However, the motors will cause magnetic interference during flight...

[adam-at-epsilon]

Some info on gyroscope drift

http://forum.pololu.com/viewtopic.php?f=3&t=6192

[stenbergd]

This issue may be considered done. We have our Kalman filter algorithms design in place. Issue #18 concerns parameter tuning. #132 concerns numerical instability of Kalman filter. We should also look at improving the practical real-time design/implementation of the filter in the code.

[adam-at-epsilon]

A weakness in the current implementation is that we have set the sample period STATE_ESTIMATION_SAMPLE_PERIOD to be 20 ms when the sensors are in fact sampled at 3 different rates.

[stenbergd]

Yes, we need to differentiate more between them by assigning individual Q, R and deltaT values, so there are plenty of implementation aspects left to consider

[adam-at-epsilon]

Ok I've implemented deltaT in fcb_sensors.c, Q and R calculations in state_estimation.c

[stenbergd]

I've noticed the state value is updated on both prediction and correction. Is this desired? Or should we only update it on correction and use a separate variable for prediction?

[stenbergd]

Todo:

• Real-time and concurrent safe solution for handling estimation (time update) and correction (sensor update)
• Initialise states to first sensor values so bias is not overestimated
• Calculate deltaT/h sample time dynamically based on last correction or prediction time (each state will have different h times based on when they were last correctied, i.e. one each needed for gyro, acc and mag updates).
• Look at how yaw angle wraps around +/-180 degrees

[superdupersvard]

Todo now:

• Calculate deltaT/h sample time dynamically based on last correction or prediction time (each state will have different h times based on when they were last correctied, i.e. one each needed for gyro, acc and mag updates).

[stenbergd]

Todo also:

• Init angleRateBias to gyroscope values
• Theoretical documentation of this new Kalman filter (Daniel will do this in the LaTeX doc)