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:

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