Height and vertical velocity Kalman filtering on MS5611 barometer: part 2

Adding accelerometer data to the Kalman filter

In my last post I wrote about a Kalman filter to take the MS5611 barometer data and derive both the quadcopter height, and the vertical velocity.  It worked reasonably well but there was a compromise between noise and latency of the filter.  To get even better results, I have incorporated now the accelerometer.  We could produce a new Kalman filter using the height, velocity and acceleration in the state vector, but it turns out we can simply amend our previous filter and include the acceleration in the control vector to improve the predicted state.


The lines:

ps1 = quadprops.height  + timeslice * quadprops.kalmanvel_z;
ps2 = quadprops.kalmanvel_z;

are updated to:

ps1 = quadprops.height  + timeslice * quadprops.kalmanvel_z + 0.5*timeslice*timeslice*quadprops.measured_az ;
ps2 = quadprops.kalmanvel_z + timeslice*quadprops.measured_az;

Where quadprops.measured_az is the World frame vertical (Z-axis) acceleration in cm/s/s.  We need to rotate the accelerometer vector from the quadcopter body frame to the World frame, using our knowledge of the pitch and roll angles (which are themselves derived principally from the gyroscopes).  The World-z acceleration (World_az) can then be converted to SI units using:

quadprops.measured_az = 100*9.81*(World_az-gravity)/gravity; // Units are cm/s/s

Body acceleration to World acceleration

First though, how big a problem is this? If the quadcopter is normally angled between say, -20 and +20 degrees, can't we just use the body z-axis acceleration? How badly can it be out?  Below I have simulated the quadcopter pitching and rolling between -45 and +45 degrees, calculating what the error in our acceleration would be if we just used the body-frame z-axis acceleration.


It's really bad! As the quadcopter tips, the colours correspond to the acceleration that would be ERRONEOUSLY measured as vertical acceleration, but is merely due to the quad orientation changing.  This is what we have to correct for.  So how can we rotate the body acceleration vector into the World coordinates?  Since my pitch and roll are based in an XYZ coordinate frame, I simply can reverse the order of rotation operations, or transpose the rotation matrix.  A bit of maths gives us an expression to find the World z-axis acceleration:

# Python code
# pitchr and rollr are the pitch and roll in radians
# ax, ay and az are the accelerometer values for x, y, and z-axis (in the body frame)
      rx = ax*-math.sin(pitchr)
      ry = ay*math.cos(pitchr)*math.sin(rollr)
      rz = az*math.cos(rollr)*math.cos(pitchr)

      World_az = rx + ry + rz

In this simulation, the body ax, ay and az were generated from the pitch and roll angles using the following expressions:

# Python code
        az = math.sqrt((gravity*gravity)/((1+math.tan(pitchr)**2)*(1+math.tan(rollr)**2)))
        ay = az * math.tan(rollr)
        ax = -math.sqrt(ay*ay+az*az)*math.tan(pitchr)

Using the math functions sin and cos we can perfectly correct for the quadcopter orientation (blue square figure above) - good job!  Sine and cosines library functions are slow however (timings), especially on an 8-bit AVR.  Given the orientation won't change much from horizontal, can we take advantage of small angle approximations to give us a faster body->world rotation? Yes we can!  I have implemented two approximations here, the first using a 1:1 identity for sin and using a quadratic to approximate the cosines.  Secondly I have implemented an additional term, - (angle^3)/6, to the sine approximation.

First approximation:
      sinpitch = pitchr
      sinroll = rollr
      
      cospitch = 1-(pitchr*pitchr/2)
      cosroll = 1-(rollr*rollr/2)
    
Better approximation:
      sinpitch = pitchr - (pitchr**3)/6
      sinroll = rollr - (rollr**3)/6

Putting it together:
      rx = -ax*sinpitch
      ry = ay*cospitch*sinroll
      rz = az*cosroll*cospitch

      World_az = rx + ry + rz

How do these faster approximations fair in terms of error?  The error surface for the first approximation is on the left and the second approximation is on the right.  
The first approximation (left) provides a reasonable approximation up to + 20 degrees and is much better than using the body acceleration without correction.  The second one looks much better again with a very low error, especially covering the pitch and roll angles of normal quadcopter use.

Results

So now we know how to rotate the z-axis body-frame acceleration into the World-frame, and how to use this in the Kalman filter, what do the results look like?  The Kalman filter works really nicely, it's like magic and it's quite amazing to see how much adding the noisy acceleration signal has improved the noise and the latency of the filter.

Moving onto the Quadcopter

The code runs well and quickly on the quadcopter - even with the noise from the accelerometers with the motors running, we still get stable and responsive readings for the vertical velocity.  All that's left to implement vertical control is to wrap a PID loop or two around the height and velocity...!  I went through several options for vertical velocity determination and by far the Kalman filter has been the optimal solution - they're magic!  I want to have the quadcopter run as autonomously as possible - so I can set a height or a vertical velocity and the quadcopter flies appropriately.

Comments

Popular posts from this blog

Getting started with the Pro Micro Arduino Board

Arduino and Raspberry Pi serial communciation