# 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:

• 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.

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.

# Pardon? Too noisy, can’t hear you!

The difference between Chloe’s flight in the video, and the ones I tested the day before is the digital low pass filter, or to be more specific the accelerometer’s dlpf since the MPU-9250 provides independent filters for gyro and accelerometer data.

This isn’t the anti-aliasing filter which is applied prior to digital sampling of the analogue  accelerometer at 1kHz – the dlpf is aimed solely at getting the data you want, and not the noise you don’t.  I don’t know what the anti-aliasing filter is set to, but it must be less that 500Hz in the same way music is sampled at 44.1kHz even though what’s nominally audible is below 20kHz – there’s a high-order low-pass anti-aliasing filter just above 20kHz,

Anyway, for the previous flights, Chloe’s accelerometer filter was set to 184Hz on the basis that the higher the filter frequency, the lesser the delay in processing the filter (5.8ms according to the spec).  I dropped it to 92Hz (7.8ms delay), and the improvement was dramatic.

And that suggests the primary cause of my drift now is possibly noise: higher frequency vibrations from the motors (for example) are irrelevant to the flight speeds, but could cause errors in velocity integration because the pass band of the DLPF is set too high.

There’s clearly a balance however which is that if the DLPF is set too low (41, 20, 10 and 5Hz are the other options), while the noise will be filtered out, the risk of filtering out the good stuff increases as does the lag (11.8, 19.8, 35.7 and 66.9ms respectively) i.e. how out of date the filtered data is!  Perhaps that’s where Kalman steps in as it offers a level of prediction (so I’ve read) which could overcome this lag.  But for now, Kalman can continue sitting on the back-burner while I play with the dlpf values to find that magic number between noisy and necessary data samples.

# 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

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

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

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!

# Elephant skeleton in the cupboard

I’ve run out of options – I think I can no longer ignore the Kalman filter.

First though a step back: I found what I’d remembered about converting gyro rate in the quad frame back to the frames used by the rotation matrix.  It’s a really nice easy read, and accurate; but there’s once critical omission – it relies on knowing the angles to convert the gyro rotation rate and hence gyros.  There’s also this which explains why minor errors in angles can lead to major errors in distances, which is the problem here.  So I think this again is a dead end.

Which brings me back to the Kalman filter to remove net acceleration from accelerometer readings, resulting only in distributed gravity readings, which can then be used to calculate angles.  To me, this still all seems like black magic, but everything I’ve read about it does suggest it is black magic, and not to worry about it, just use it.  Although that’s anathema to my whole project, I have no choice.

# An itch that needs scratching.

It’s bothered me for a long time that the two sources of angles fed into the complementary filter do not have the same meaning.  Since adding the rotation matrix code this has been increasingly evident.  Finally with the graphs from yesterday, and the consistent forward drift, understanding this has become a top priority.

The nub of the problem is the difference in the context of the angles:

Euler angles are cumulative: they represent the rotation of gravity between the inertial reference frame (earth axes aligned with north, west and up) and the quadcopter X, Y, and Z axes respectively.  To perform the rotation each angle is applied in sequence.

Integrated Gyro angles are independent – they are all in the quad frame, and represent rotation rate around each of the quads axes all within the quad reference frame.  Gravity doesn’t enter the equation.

So currently the complementary filters is trying to merge two different types of angles and can only produce garbage as a result.

I may to convert gyro angles into Euler angles, and for a change I found a site which explained these well, and in clear language and equations that don’t obfuscate the facts behind fancy symbols.

Euler vs Gyro Angles

There’s a few other links from that page that explain other parts of the quad code really clearly too.  Well worth reading.

Or I may need to use Kalman to smooth out the Euler angles, and dispose of the complementary filter altogether.

Or there may be a plan C that I’ve not had time to think of yet.

But I’m stuck on a bus for 2 hours tomorrow, so I’ll have plenty of time to muse and snooze!

# 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:

# Angelic Angles Algorithm

Currently, Phoebe can only run short flights safely; flights more than a few seconds lead to inaccurate angle of tilt calculations due to drift over time in the integration of the gyro angular speed.  The net result of a longer flight would be Phoebe thinking she’s hovering horizontally, when in fact she has a slight angle of tilt accelerating her towards a brick wall..

I’ve mentioned in the past that angular information is also available from the accelerometer by using the Euler calculation, but short term it’s prone to the noise I spent months trying to reduce.

I’ve also mentioned previously that by applying a mathematical filter to these two sources of angular data, you can extract the best of both worlds both short and long term.

But I’ve held off doing so through lack of understanding how they work.  I hate the idea of copying someone else’s code without understanding it, since then I can’t recognise / fix bugs in that code, or tailor it to be a perfect fit for my code.

The best of the filters commonly used is by Kalman (again previously mentioned) – this still remains too complicated for my aging and damaged brain to comprehend.

However, I’ve just found this article which means I now fully understand how a complementary filter works, and so I’m happy to deploy the filter in my code.

P.S. The previous set of Chapters and Appendices aren’t yet complete, but with winter upon us and the nights drawing in, testing time is in even more short supply.  I hope for one final Appendix, but until then, life (both mine and Phoebe’s) moves on.

# Chapter 2: Dimensional shifts

Given that I’ve chosen the 2 PID option for drift management, why are the angles (integrated from the gyro angular speeds) still necessary?

Imagine Phoebe is horizontal, her X, Y, and Y axes aligns with the earth’s X, Y, and Z axes.

But imagine that she’s pitches over at an angle θ, and rolls over at an angle ψ in the Y plane. Then her X, Y, and Z axes do not align with the earth’s X, Y, and Z axes; readings from her accelerometers are relative to her axes, not those of the earth.

Now the “target”s for the horizontal and vertical speed PIDs make most sense in the earth space – for example vertical speed “target” = 0, horizonal speed “target”s (both X & Y) = 0 means a stable, non-drifting hover regardless of wind or other factors.

Therefore the PIDs controlling vertical and horizontal movement need to be fed sensor inputs converted from Phoebe’s space to the earth’s.  And that’s where the angles come in.

Conversion of Phoebe’s X-axis sensor to the earth equivalent is like this – simply imagine a triangle between the 2 X axes – the value of movement compared to the earth is less than that detected by Phoebe thus:

Xe = Xp * cos(θ)

Likewise conversion of Phoebe’s y-axis sensor to the earth equivalent is like this:

Ye = Yp * cos(ψ)

Now given that Phoebe is tilting on both the X and Y axes compared to earth, the conversion from Phoebe’s Z axis accelerometer readings to the earth’s is like this:

Ze = Zp * cos(θ) * cos(ψ)

θ and ψ can both be inferred by integrating the gyro sensor outputs.  However this use of angles between Phoebe’s and the earth’s spaces makes the accuracy of the angles critical. Integrated gyro output is good enough for short flights, but as flight lengths increase, rounding errors in the integration are likely to make the inferred angle increasingly wrong. That’s where Kalman filters or Complementary filters are used to limit this drift.

For now though, I think I have all the pieces of the jigsaw I need to solve wind-drift in short term flights, and as an added benefit, take-off from non-horizontal platforms, and controlled horizontal movement – I’ll leave Kalman / Complementary filters for another day.