Accelerometer digital low pass filter

Yet more indoor testing, indirectly GERMS related, more aimed at investigating drift against the accelerometer dlpf setting.

Speculatively, the net is that

  • alpf of 3 or less (>= 41Hz) means there is no vertical drift, but there is horizontal drift because real X- and Y-axis noise is not being filtered out and the motion processing thinks there’s X- / Y-axis acceleration when there isn’t.*
  • alpf of 4 or higher (<= 20Hz) gives no horizontal drift, but there is vertical drift because real Z-axis acceleration is being filtered out as noise meaning the motion processing thinks there’s less acceleration than there is.*

Again speculative, I think X- and Y- axis noise was due to unbalanced props leading to asymmetric noise which when integrated leads to non-zero drift velocity: if one of the props has slight damage, this will generate the assymeteric noise very well.

My props do have nicks and muck on them, so I cleaned them up, and tried alpf 3 again, but the drift still remained – to some extent expected as the cleaned props still had nicks in them.  So out came a set of unflown props.  Balanced as best I can, I tried again at alpf 3. Some vertical drift had appeared unexpectedly, so I tried alpf 2, and it was perfect.  So that’s what I’ll be sticking with.

The only down side of this is that the noise getting through at alpf 2 means my germs idea in its basic form is a no go.  It’s shelved for the moment as it’s no longer necessary for my short flights, and instead, I’ll see how far I can extend the complementary filter to see how long a zero drift flight can be achieved.


*To be more precise, it the integration of the acceleration (i.e. the velocites) that are wrong. The Z-axis velocity is greater than zero in reality but zero according to the motion processing due to the acceleration that had been filtered out. In contrast, the X- and Y- velocities are zero according to motion processing, but drifting in reality as the dlpf is letting through asymmetric noise.

 

Akin to Kalman

I had another quick go at understanding Kalman filters yesterday, and mostly failed miserably with one exception.  I think what I current have is working in a similar way to a kalman filter:

 #-------------------------------------------------------------------------------------------
 # Collect the latest batch of data from the sensors, and sort out units and calibration
 #-------------------------------------------------------------------------------------------
 qax, qay, qaz, qrx, qry, qrz, i_time, temp = sampling.collect()
 qax, qay, qaz, qrx, qry, qrz = mpu6050.scaleSensors(qax,
                                                     qay,
                                                     qaz,
                                                     qrx,
                                                     qry,
                                                     qrz)


 #------------------------------------------------------------------------------------------
 # Track the number of motion loops and sampling loops; any discrepancy between these are the
 # missed samples or sampling errors.
 #------------------------------------------------------------------------------------------
 motion_loops += 1
 sampling_loops += i_time * dri_frequency

 #-------------------------------------------------------------------------------------------
 # Angular predication: Now we know the time since the last batch of samples, update the
 # previous angles with the 'integral' of the previous euler rotation rates.
 #-------------------------------------------------------------------------------------------
 urp, urr, ury = Body2EulerRates(qry, qrx, qrz, pa, ra)
 pa += urp * i_time
 ra += urr * i_time
 ya += ury * i_time

 #-------------------------------------------------------------------------------------------
 # Based upon the predicted angles, rotate the new accelerometer data to earth frame.
 # Next, run the earth frame acceleration through the Butterworth LPF to extract gravity.
 # Finally, rotate revised gravity back to the quad frame.
 # This provides the best guestimation of current gravity orientation and value.
 #-------------------------------------------------------------------------------------------
 eax, eay, eaz = RotateQ2E(qax, qay, qaz, pa, ra, ya)

 egx = bfx.filter(eax)
 egy = bfy.filter(eay)
 egz = bfz.filter(eaz)

 qgx, qgy, qgz = RotateE2Q(egx, egy, egz, pa, ra, ya)

 #-------------------------------------------------------------------------------------------
 # To stop integration drift from the gyros, merge the short-term noise free predicted angles
 # with the long-term accurate acclerometer which short term are tainted with acceleration as
 # well as gravity. tau is the period during which we expect acceleration to average out leading
 # only the net gravity value. tau can be small if acceleration is short and sharp. There is
 # a balance here between the period to trust the integrated, reorientated gyro readings and
 # the period where accelerometer spikes average out.
 #-------------------------------------------------------------------------------------------
 upa, ura = GetRotationAngles(qax, qay, qaz)
 tau_fraction = tau / (tau + i_time)
 pa = tau_fraction * pa + (1 - tau_fraction) * upa
 ra = tau_fraction * ra + (1 - tau_fraction) * ura

Breaking this down it:

  • reads the sensors
  • predicts the current pitch, roll yaw based upon the previous iteration’s angles and the gyro changes since
  • extracts gravity from the accelerometer readings using these predicted angles to orientate accelerometer readings to the earth frame and pass the acceleration  through the Butterworth IIR filter
  • fuses accelerometer angles with these latest angles.

I’m not happy with the section in red – there’s a choice here neither of which I like much:

  1. the version shown uses a complementary filter with a fixed value of tau and a time increment to fuse accelerometer angles with the best predicted angles: the problem here is that the time increment doesn’t take into account the sharp acceleration peaks and troughs in the flight plan. Instead it relies on the fact that overall these peaks and troughs will even out but in the short term real acceleration seeps into the measure of gravity and drift ensues.
  2. the alternate version does away with the complementary filter and instead uses the qgx, qgy and qgz to get the latest set of angles from the red call the GetRotationAngles: the problem here is that errors accumulate over time as there’s no feed from real accelerometer readings to curtail the integration errors which also will ultimately lead to drift.

Which leads to option 3: tau is tuned dynamically based upon the deviation from a stable value of accelerometer readings rather than incremental time – something along the lines of this:

 upa, ura = GetRotationAngles(qax, qay, qaz)
 tau_fraction = tau / (tau +  gain * ((qax - qgx)2 + (qay - qgy)2 + (qaz - qgz)2)0.5)
 pa = tau_fraction * pa + (1 - tau_fraction) * upa
 ra = tau_fraction * ra + (1 - tau_fraction) * ura

More thinking and testing needed to actually work out the right way to dynamically tweak tau on the fly.

The similarity with a Kalman filter is that it’s 2 stage, prediction and fusing, where the fuse fraction depends on an estimated accuracy of the value to be fused. Hence the raw acceleration value always gets through but how much gets through depends on how dependable the value is thought to be – i.e. it’s deviation from expectations.


ADDENDUM
For clarity I’ve rewritten the bit of speculative code to now remove tau and instead refer to the fusion_fraction:

 upa, ura = GetRotationAngles(qax, qay, qaz)
 fusion_fraction = 1 / (1 + gain * ((qax - qgx)2 + (qay - qgy)2 + (qaz - qgz)2)0.5)
 pa = (1 - fusion_fraction) * pa + fusion_fraction * upa
 ra = (1 - fusion_fraction) * ra + fusion_fraction * ura

So when there is net acceleration in the system, the RMS error

gain * ((qax - qgx)2 + (qay - qgy)2 + (qaz - qgz)2)0.5

is larger, meaning the fusion fraction is smaller, and a smaller fraction of the raw accelerometer value is used to update the angles. The first step of testing is simply to get an estimate for gain based upon the tau value (0.5 seconds) that’s worked well in the past.

Phoebe’s Playtime in the Park

I took Phoebe down to the local play park to try some extended flights without the risk of her crashing into solid objects – I’m on my last set of props and I really can’t afford to keep replacing them several times a month.

Anyway, net of the flights was she’s good for a few seconds as shown in yesterday’s video, but extending the hover time to 10s from yesterday’s 4 shown accelerating drift.

Best guess is longer term accuracy of angles using just integrated gyro reading, and I need to reintroduce the complementary filter to merge in the angles from the accelerometer to provide longer term stability.

Minor update

I’ve added the gravity complementary filter and a few other bits to hopefully work together with it:

  • dlpf is now set to 2 – 94Hz with 3ms lag (from 3 – 44Hz and 4.9ms lag)
  • motion processing frequency of 71Hz (from 43Hz)

The aim of the first change is to ensure we get to see the acceleration spikes, so the filter can take them out of gravity.  We can’t let the dlpf take them out as the motion processing needs to see them to calculate net acceleration with gravity removed.

The aim of the second change is to increase the resolution of the motion processing cycles so that fewer cycles are impacted by acceleration spikes.

Both of the above changes are only possible due to the recent increase of speed in the code from ∼450Hz to ∼900Hz.

Net result though wasn’t significant in the first test flight – same climb well above what the flight plan defined.  Not a problem, I realized it would need tuning.

What was a problem is the battery bank I use to power Phoebe.  It appears they don’t like impacts.  I’ve gone through 4 now – it seems as though the USB B socket gets detached; the bank charges fine, but there’s no power from the USB B socket.  Ironically, they are beautifully made, and I can’t find a way to open them up to fix the problem.

I could do with a super low profile USB B plug, but I’ve never seen such a thing.

Instead another tenner to Amazon for yet another battery bank.

Accelerometer drift proof and solutions

These are readings direct from the 3 accelerometers with virtually zero code tinkering – specifically no subtraction of gravity; the only code modifying the sensor data values is fixed calibration values, fixed unit conversion and averaging. None of these three should cause the decline in the Z-axis (yellow) line against the right-hand axis.

Z-axis drift

Z-axis drift

So I need to do something about this, in particular the Z axis which is the cause of my ever climbing flights.  3 approaches:

  1. give in and add an altimeter – but I’m waiting for the next revision of the DroTek board that has the pin spacing I needs – however this wouldn’t be ideal for indoor flights due to air pressure changes as people open doors etc
  2. give in and add ultrasonic range finders – again not perfect for a variety of reasons – firstly it’s I2C is only 100kHz rather than the 400kHz for the MPU6050 so could slow things down without some very careful coding;  secondly it’s tracking distance to ground where as the accelerometer is tracking from takeoff point – so in a lecture theatre, takeoff from a table would give horizontal flight from the accelerometer, but a rapid descent as the quad passes over the edge of the table, also possible causing two of the arms to clip the table; third and finally, I’m not sure how well a URF will perform on grassy rather than hard surfaces.
  3. filter the accelerometer output to smooth the acceleration (the spike at one second in the graph above) but trace the steady drift downwards; simplest would be yet another complementary filter, merging in fractions of latest accelerometer readings into the initial value in the earth axis; better though would probably be a Kalman filter where the prediction it provides may well be able to remove the spikes completely – it’s also easier as there’s just a single source of the data being filtered, unlike the current complementary filters which are used to merge angles from gyro and accelerometer.

So option 3 it is, though as always, I’ll chicken out on the Kalman unless the complementary doesn’t work well!

Wot no Kalman!

Before I’d even finished my previous post, I’d found a solution that meant I didn’t need a Kalman filter just yet

It all started with this, describing how to merge accelerometer Euler angles with gyro angular rate transformed from body frame to the 3 Euler frames.  But as per usual, it wasn’t enough; while I understand the complementary filter well, I didn’t understand the conversions of gyro rotation rates in the body frame to the Euler frames.  Until I found this.

That now allows a complementary filter to track Euler angles short term via gyro and long term with the accelerometer.  I know I’ve done this before, but this time, I’m merging measurements in the same same set of Euler frames so the results should be correct.

A bit of manic coding last night, combined with a repeat of yesterday’s tests showed this.

Complementary Euler

Complementary Euler

Compare this with the graph from yesterday’s equivalent tests:

net acceleration

net acceleration

I hope you agree this is much better – as expected there are roughly sinusoidal lines about 90° out of phase as Phoebe rolled around the floor in circles.

No option now but to test this new code live – need to find where I left my bravery first though!

What do you get if you cross an accelerometer with an ultrasonic range finder?

Fusion.

Pink is the range finder distance differentiated against time giving velocity in the quad Z axis – note how spikey it is.

Green is the accelerometer output integrated over time giving velocity in the quad Z axis – note that it’s drifting downwards between 6 and 10 seconds when the quad is actually hovering – I know because I’m holding it in my hands.

Orange is what you get when you fuse these together with a complementary filter – note it doesn’t drift downwards, and much of the spikiness is gone.

Velocity fusion

Velocity fusion

Time for real test flight tomorrow – weather is looking good too.

Business as usual

Crass bug fixed leads to the complimentary filter smoothly tracking the noisy angles from accelerometer:

Angles tracking

Angles tracking

The complementary filter almost certainly needs refinement – I think the level of smoothing is too high. At the same time, I suspect I can drop the dlpf from its current 20Hz. Together that should help.

But at the same time, a test flight still showed Phoebe drifting forwards in the real world due to sensors reading backwards movement the sensors’ world.

P.S. Interestingly, the angles suggest Phoebe thinks she’s level, which would then suggest the sensors are misaligned compared to how they were when calibrated.  Must check to see if they’ve worked themselves loose since calibration – that would certainly account for pitch drift, especially as the pin layout would work itself loose along the X / pitch axis,

P.P.S The bug fix is now on GitHub.

Compare and Kontrast…

There’s a passing resemblance between the complementary and the Kalman filters, so to start understanding Kalman, I changed variable names to ones I recognised.

Complementry:

Xn = gain * (Xn-1 + (gnow - gprev) * δt) + (1 - gain) * an

Kalman:

Xn+1 =  gain * Xn-1 + (1 - gain) * Xn

The main differences are that the complementary filter is merging two sources of the same information based on a gain (< 1) whereas the Kalman is merging the previous processed and current sensor readings to predict the next value where the gain is dynamic but still less than one – and I didn’t get much further than that, because in writing this down, I spotted a crass (krass?) error in my complementary filter which could more than account for the flight problems.

Kalman will have to wait another day to shine!

I’ll update the code on GitHub as soon as it’s tested – the weather tomorrow looks to be in my favour.

Complementary vs. Kalman

Thanks to Mark, and an outer-blog-space e-mail conversation I had with him, I think now I have just enough interest and need for a Kalman filter that I’ll do some proper research and thinking.  I’ll explain why but first, a recap on the complementary filter:

Angles of pitch and roll can be calculated from both the gyro and the accelerometer.  For the gyro, you integrate the rotation rate; for the accelerometer, you assume the only force is gravity and use the distribution of gravity across the sensors plus a little trigonometry.

Both methods have flaws

  • the gyro results drift – I never have found out why; whether it’s integration errors, or inherent to the gyro itself, perhaps due to its angular momentum.  Regardless of which way, you can only trust the angles from its integration short term.
  • the accelerometer results are plagued with noise and cease to be accurate when forces other than gravity come into play i.e. acceleration from the props and drag / wind resistance, but longer term, these average out, yielding accurate values.

The complementary filter does a rolling ‘merge’ of these values, with a short-term bias on the gyro against a long term background provided by the accelerometer.

This has worked very well until I moved away from controlling Phoebe with angles to controlling her with motion – the reason behind the move (pun intended) being angles could never be used to counteract drift in windy conditions.

Anyway, how could the Kalman filter do a better job?  It provides an iterative way to filter out noise and hone in on the ‘correct’ value in a predictive way.  There, you now know exactly the same as I do!

That means I can stop using the gyro for angles, retiring it into just a rotational stability role.

The way the Kalman approach would work is a bit like this: you may remember the code loops at about 450Hz, but only processes the resultant integrated / averaged data at about 40Hz currently?  If instead of averaging @ 450Hz, each accelerometer sensor read is fed through a Kalman filter then at the 40Hz processing point, it’s the refined value from Kalman filter that is used for motion processing. That should remove noise and reduce the effect of non-gravitational forces on the sensor output.

2 problems I see initially:

  1. the basic Kalman filter requires inputs at a fixed frequency; that’s fine as that’s how the MPU6050 makes its data available.  But currently I have it set to 1kHz to make sure every sensor read is new but that means sometimes the code loop running at 450Hz will miss an output from the sensors – not a problem for integrating as I track sample time closely; but for Kalman I’d have to drop the 1kHz down to 333Hz to ensure the values fed into the Kalman are all equally spaced at 3ms – not really a problem except I’ve worked so hard to get the loop speed as high as possible, and now I don’t need that!
  2. More importantly, I really don’t understand how the filter works in any detail whatsoever.  The code I’ve seen is simple but until I understand what it’s doing and how it works in intimate detail, I’m not letting it anywhere near Phoebe.

Here’s a few of the links Mark pointed me at: