Discussions

I've been working for a while to improve the ability of DCM in ArduPilot to accurately track gyro drift while at the same time making it less susceptible to noise. It is a delicate balance, but it is a worthwhile thing to improve as it makes such a big difference to how planes and copters fly in practice.

The recent releases of ArduCopter and ArduPlane included a number of DCM changes that came out of this work. The most important ones were:

• slope limiting the changes in omega_I
• scaling the kI term according to the known sensor properties
• using the 'drop z' method to reduce the impact of noise

Each of these changes came out of studying the impact of large amounts of sensor noise and gyro drift on the behaviour of DCM in APM. The first two were fairly uncontroversial, but the 3rd one left some people wondering why it worked (or if it worked!). That is a reasonable question, as we never really put it on a sound mathematical footing - it worked well in the simulator and pilots said it helped in practice, so it went in.

Unfortunately it led to a side effect discovered by Gábor, which is that poor accelerometer calibration and scaling led to inaccurate attitude at larger roll/pitch values. It worked fine for most people, but on some APM1s the attitude error could be quite large.

I've now found a much better way of coping with large amounts of noise while still tracking gyro drift, so I will be 'dropping' the 'drop z' method in favour of the new approach.

First some background. The drift correction in DCM aims to cope with the fact that gyros aren't perfect. If we had perfect gyros and we ran DCM at a very high rate then we wouldn't need drift correction at all, and we could keep an accurate attitude estimate just by updating the DCM matrix using the gyro vector. Unfortunately gyros aren't perfect, so we need a reference vector to do some correction of errors that build up over time.

There are really two key types of errors to worry about. One is short term errors that result from imperfect application of the gyro vector to the DCM matrix (for example, from the fact that the gyros don't have infinite precision - they measure rotation in units of about 0.4 degrees/second). The second type of error is long term drift of the gyro zero point.

We cope with the first type of error quite easily by using the accelerometers and compass (or GPS) as a reference vector to 'push' the solution in the right direction.The amount of push is re-calculated every time we get a new reference vector, and all we really need to do is choose a reasonable scaling factor for this push so that the time constant is reasonable. A higher value will push the attitude to line up with the accelerometers and compass more quickly. This value is called kP in APM.

Coping with the second sort of gyro error - gyro drift - is much harder. This sort of error happens when the gyro starts reading something like 3 degrees/second rotation when in fact the aircraft is not rotating about that axis at all. That scale of drift overwhelms the ability of the short term 'push' term to cope with the error, and the result is a very bad attitude solution.

Newer gyros (like those in the MPU6000 used in the APM2) suffer from gyro drift much less than the older gyros in the APM1. The APM1 gyros can drift at a rate of around 2 degrees/second/minute, which can quickly build-up to be far too much drift for the short term error correction to cope with.

The original paper on DCM by Bill Premerlani provides the basis for coping with this gyro drift that we use in APM. The amount of gyro drift is estimated in a vector called omegaI, with the error at each update of the DCM matrix used to slowly push omegaI towards the right value. The omegaI is an integrator - adding up the errors happening with each step to produce the long term drift estimate. The omegaI value is then used to 'correct' the hardware gyro values, by subtracting off the drift estimate. The constant used to determine how fast this drift is learnt is called kI.

Unfortunately it is very hard to get this integrator learning right. If it learns too slowly then the DCM code won't cope with the gyro drift that may happen in a real APM, and the result is a poor estimate of roll and pitch as the kP term struggles to cope. The aircraft starts to lean over badly. On the other hand, if it learns too fast, then it may mistake sensor noise for gyro drift and incorrectly 'learn' gyro drift that isn't there. That is just as bad, and results in leans as well, in the opposite direction.

It is this second effect, where large amounts of noise can lead to omegaI build-up that I was particularly interested in. Some flight logs showed it was a very real effect in some aircraft. It doesn't happen in every flight, but when it does happen it can easily lead to a crash.

Here is a plot of omegaI being learnt in the SITL simulator when we have a noise level of '2' (thats the sort of noise level where you start to think the aircraft will fall to pieces with vibration).

The 'sawtooth' line is the simulated level of gyro drift which DCM is trying to learn. It is set to 1 degree/second/minute, with a period of 10 minutes. That is quite a high drift level, but is quite possible on an APM1. The 3 omegaI lines show what the DCM learning of the gyro drift did - they managed to track the simulator drift, but not very precisely. In fact, this is much better gyro drift tracking than what we achieved in earlier versions of APM!

The graph below shows the same test, but with a small tweak to the omegaI learning code:

That is much better! The three omegaI terms have tracked the simulated gyro drift much more accurately.

You may notice that the 2nd graph is quite granular, whereas the first graph is relatively smooth. That is because what is being done with the new approach is to save up omegaI changes over a period of 10 seconds, and then apply them all at once.

The reasoning behind this change is twofold. The first is the interaction of the slope limit for omegaI. We introduced a slope limit to omegaI as part of the first round of noise robustness changes. The way it works is that it limits omegaI changes to be at a rate no higher than what the sensor driver claims is possible with the gyros being used. For the APM1 that is 3 degrees/second/minute, and for the APM2 it is 0.5 degrees/second/minute (see libraries/AP_InertialSensor in the get_gyro_drift_rate() functions).

The slope limit helped reduced the impact of noise by preventing error spikes from noise from pushing omegaI faster than the gyros really can drift. We're trying to learn a physical parameter in omegaI (the gyro drift), so limiting it to the physical limits of the device makes sense.

The problem is we were applying the slop limit at the drift correction rate (50Hz for APM and 100Hz for ACM). This applied the limit to each small step, but the line isn't smooth, so the slope limit had a much larger effect than it really should.

The second problem was one noticed by Jonathan Challinger. Jon noticed that omegaI would react over the short term to errors induced due to aircraft turns. The aim of omegaI is to capture long term drift, but it was in fact interacting with the P term in the usual way a PI controller does, and capturing some of the short term error.

So instead of applying omegaI changes directly into omegaI, we instead apply them to omegaI_sum. That vector builds up over a 10 second period, then we apply the slope limit, and add omegaI_sum to omegaI. The omegaI_sum vector is then zeroed and the process starts again.

As you can see from the above graph, the difference in ability to accurately track gyro drift is quite dramatic!

You can see the patch in my DCM-wip branch. After this is flight tested in both APM and ACM I expect it will go into a upcoming release.

Cheers, Tridge