I have worked out a new way to account for acceleration in the computations to detect roll and pitch gyro drift in IMU computations.
The method that is commonly used right now is based on computing centrifugal effects in the body frame of reference. This method requires an aerodynamic model of the aircraft that can be used to compute acceleration in the body frame using the rotation rate vector measured by the gyro, and the air velocity vector. As a result, this method has small errors when used with a fixed wing aircraft, and has large errors when used with a multicopter.
The new method, described here, does not require a model of the aircraft at all, so it is an exact method, not an approximate one. It is based on three innovations to the currently used method:
1. The roll-pitch reference vector is acceleration minus gravity, instead of gravity.
2. The calculations are performed in the earth frame, so there is no computation of centrifugal effects needed at all.
3. The computation uses a formulation based on integrals, which attenuates the effects of noise.
I have not yet tested this method, but I am confident that it will work. I will report back, and post a blog entry, when I have some test results.
Thanks for another great piece of work Bill!
I've done an initial implementation of this idea for ArduPlane/ArduCopter in my accel-wip branch
(note, I don't recommend anyone actually try to fly a real plane or copter with the code from this branch yet - please wait till more simulation work has been done)
The main code for the algorithm is in libraries/AP_AHRS/AP_AHRS_DCM.cpp in the function drift_correction(). The code includes a few minor changes from Bills paper:
Thus far Bills idea seems to work very well unless a yaw error develops. I've done most of my testing with a simulated gyro drift rate of 2 degrees/second/minute on all 3 axes, plus about 0.25g of white noise on the accelerometers and 20 degrees/s white noise on the gyros.The gyro drift is setup in a 'sawtooth' pattern, reversing every 5 minutes, so it rises at 2 dps/min for 5 minutes, then falls at the same rate for the next 5 minutes and so on. This is an easy pattern to recognise in the logs.
If I use the GPS as the primary yaw correction source then the flight is very good - better than what we have achieved previously. The omegaI drift correction tracks the synthetic drift rate very well.
Things don't work nearly as well if I use the compass as the primary yaw correction source. The flight starts off fine, but after 5 to 10 minutes three things start to happen at about the same time. The DCM yaw starts to diverge from the true yaw, the DCM pitch starts to diverge from the true pitch, and the omegaI.z drift starts to diverge from the true drift.
It is quite possible that the yaw drift correction we use in APM is just not good enough. I'm chasing down some ideas on improving it and I'll re-test once I've done that.
The main idea I'd like to try is to use the same 'integrate in earth frame' idea that Bill has introduced with this paper on the magnetometer. The same basic principle should work with the magnetometer, except it will primarily give information on the yaw error instead of the roll/pitch error. I'm hoping it will provide a more reliable yaw drift correction technique than what we use currently, which is based on applying the DCM matrix to the current mag vector and using that to directly compute a heading. The heading error is then used to build omega_yaw_P and the z component of omegaI.
A few more comments on the prototype implementation. Currently it calculates the velocity only in 2 dimensions, ignoring any vertical velocity. I think we should probably estimate vertical velocity using the barometer, especially for multicopters. I didn't use the GPS height for vertical velocity as it is notoriously unreliable.
The tests I did were with a 'perfect' compass with zero offsets, no noise and constant magnetic field strength. One of the key things I want to see is how well this new drift correction system works with a more realistic compass that is being strongly influenced by the magnetic field from the motors and power distribution board. That is probably my primary concern about this new drift correction method - because it no longer can assume that the aircraft flies along a particular axis I think it may now be much more dependent on good yaw to get the roll and pitch correction right. With the old code if the compass was behaving badly for some reason (eg. motor interference) then navigation will be thrown off, but the aircraft would still retain good stability, and a pilot could bring it home without problems in FBWA mode. With this drift correction technique if a yaw discrepancy develops between the GPS and the aircraft then that will impact on roll and pitch stability, and it may become impossible to fly at all.
So what I'd really like to find is a way to support 3D acceleration correction in multicopters and planes while still retaining roll/pitch stability if yaw is way off. If we can separate these two then we'll really be onto a great system.
On a related topic, I've finally implemented your offset nulling revisited compass offset algorithm for ArduPilot. It works very well! Here is a graph from our Rascal simulator when I setup offsets of -60, 120, -180:
I chose a gain of 0.3 for this run, although I've increased that to 0.5 for the release code.The graph shows the offsets converging nicely in a few minutes. The yellow line is the heading error between the heading that the correct offsets would give, and the heading that the current offsets give. You can see it converges on zero quite well.
I also limited the single step delta to 2, so the total offset change (as a vector length) is never more than 2 in one step. That is just paranoia in case we get some garbage from the compass for a short time (eg. a spike in throttle magnetic field effect).
I really like the fact that this new method avoids using the DCM matrix, which means no possibility of nasty feedback effects.
Thanks very much for publishing this idea!
Tridge, It's super to see Bill Premerlani's algorithm re-implemented and graphed again on another auto pilot.
In the past, alignment of the magnetometer with the Autopilot was an issue. But now, I enjoy being able to put my magnetometer way out down the wing of my plane away from motor effects. As your graph shows, the algorithm will figger out any small (or large) mis-alignment between the mag and the autopilot and correct it.
I have looked at the realignment algorithm, and I do plan on trying an implementation, but it doesn't look nearly as simple as the offset nulling algorithm is. I'd never even heard of the vex() operator before!
The idea of being able to automatically cope with 180 misalignment of the compass is very appealing, but it will have to wait for another day.
This is taking the good way. I was looking for an algorithm to estimate attitude of race motorbikes. But none, except DCM, compensate for centrifugal forces. But even DCM desn't take in account the accel in X axis for ground vehicle.
With these new methods, and with the code from Andrew, it will now compensate for heavy accel (speed change) during long straigh and during braking. It will also work during wheelies (front wheel leave ground) and on hard braking.
I should still improve my math skills :-)
This is just an update on my attempts to implement your new roll/pitch drift algorithm. I still haven't got it working well, although I haven't given up.
I've tried redoing our compass code as we discussed, so it no longer treats it as a roll corrected compass, and instead works directly with the mag vector in the DCM code. I think that is a very worthwhile improvement, but it didn't solve the problem (drat!).
The thing that has really surprised me is how consistent the failure is. As you know, I'm a bit obsessed with noise effects, and I had thought that the failure of my implementation of this algorithm was down to noise or drift levels so I concentrated how noise interacted with the algorithm. Finally I thought to just turn off all noise and gyro drift in the simulator, and ran with 'perfect' sensors. It still crashed in the same way after 10 minutes of flight!
That graph shows the last 3 and a half minutes of the flight. There are a few notable things. The first is that it almost crashed once at 21:55:04. The plane is supposed to be flying nice circuits on a perfect windless day with perfect sensors. It tracks the roll really well until suddenly the solution collapses. That first time it recovers, but the second time it hits the ground before it can get back on track.
The second notable thing is how much 'gyro drift' it is learning. This was with perfect gyros that were not drifting at all, yet it built up omegaI.y to over 2 degrees/second by the end of the flight and omegaI.z got similarly large. I could just drop the I gain, but then I don't think it will be able to track drift at the levels we want to support for the old APM1 gyros.
I still think the key to this is yaw lock. Even without the tilt corrected compass code the error terms are highly dependent on yaw accuracy, but the yaw accuracy is highly dependent on the amount of error! It's all great if the errors stay small, but once you get error in one or the other you get feedback between them and the solution collapses. If you have enough altitude it does recover a good rotation matrix in 5 to 10 seconds or so, but at the heights I'm testing at (100m) when flying at 30m/s, those 10 seconds are often enough to crash.
I wonder if we should be avoiding using the product of the DCM matrix with the mag vector in computing the yaw drift term? We could instead calculate the minimal rotation matrix that takes the earth magnetic field and rotates it to match the current mag vector. Then use the yaw component of that rotation as the driver for the yaw lock. That would remove a feedback path in the error. What do you think? I know it's quite contrary to the usual technique of trusting the DCM matrix.
It also could just be a bug in my code (always a possibility!). Maybe there is a 10 minute "crash the plane" timer that I've not noticed before :-)
I think the key issue is error buildup in the omegaI terms. As I mentioned in my report on what happens during sustained rotations, there are some effects that "break"Robert Mahoney's proof that the PI gyro drift controller is stable. In fact, there are situations in which it is not stable.
One of the situations is sustained rotations. In that case, the PI gyro drift controller goes unstable, but it takes about 10 minutes to "break"! In my report, I analyze what is going on, and explain why it happens during sustained rotations. The solution for sustained rotations is to turn off the integrator in the drift controller.
In the case of the new roll/pitch drift compensation algorithm, there is likely some subtlety that is causing error build up, such as an unintentional bias in the the implementation. I am hopeful that you and I can work together to figure out what it is, and fix it.
I suggest before you try improving the algorithm we try to get a better understanding of why it crashes after 10 minutes.
First, the list of "usual suspects":
1. The algorithm is inherently unstable after 10 minutes.
2. Bias or bug in the simulation.
3. Bias or bug in the implementation.
4. It might have been better if we (I) had not allowed the acceleration reference vector to adjust yaw. (Actually, this is a strong suspect.) It might have been better if only the horizontal components of the cross product of the roll-pitch reference vectors were reflected back into the body frame. Taking the entire result allows the magnetometer and the accelerometer to fight each other, I think.
To help us get a better understanding, I wonder if you would be willing to do the following:
Run the simulation again until it crashes. Send me the same plots as above, except I want to see the entire history, from initialization to crash. Also, I would like to see OmegaP, and also the results of the two cross products involved in the drift compensation. (cross product of known and measured earth mag field, and cross product of accelerometer integral with gravity minus acceleration.)
Also, just to be sure on one aspect of the implementation....the integral of R*A (dcm matrix times accelerometer) should be reset for each GPS report. In other words, the integral is performed between GPS reports, its not accumulated from the very beginning. If it is accumulated from the beginning, the algorithm will certainly eventually go unstable, probably after about 10 minutes.
Another test that would be useful: deliberately initialize the matrix with deliberate roll, pitch, and yaw errors, and see if the drift correction recovers. For this test all I would want to see would be about 1 minutes worth of data.
Answering your comments in reverse order - sorry about that!
I did try the 'does it recover from bad attitude' test. I use a little script called fakepos.py to do that. That script creates UDP packets in the same form as the JSBSim simulator and sends them to the SITL system, but allows me to setup known inputs. For example, I can tell it to suddenly change the attitude to a 45 degree roll, without any gyro changes. The PI controller then needs to drift correct it to get the right answer. Similarly I can setup a sustained rate of rotation in any direction. I tend to use this script to help me tune the PI controller constants to get the time constant I'm looking for.
It passed those tests perfectly, recovering from arbitrary attitude errors with no problem. That tells me that I haven't (for example) got a sign wrong in the code.