Team,
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.
Best regards,
Bill Premerlani
Replies
Basic gyro drift compensation only requires accelerometer and magnetometer, both of which generally function indoors. This is used in the absence of good GPS data.
Dear Bill, I am trying to build a framework for quadcopter that try to superposition different functions rather than calculating them all together.
I hope you give me your opinion.
One of the problems with the APM and using this in the existing APM/APM2 systems is the GPS that 3dRobotics sells with the kits. In order for this to work well the GPS must output velocity info in all three axis. The current special released version of the mediatek gps sold by 3dr does not have this output available. Its not available via either the custom message or via and NEMA sentence.
I've been doing some other work and I've been using the 20Hz Venus GPS from spark fun if you use the high dynamics software load (available on their product page) and enable binary messages(using the gps viewer software available on SF web page) and no nema messages it spits out the following frame...
I've padded the frame with the preamble and end for my own code....
Please note that it spits this out in Bigendian format so arm's or pc's will probably have to do htonl or similar to the values.
typedef struct {
BYTE Preamble[5]; /*A0A1003BA8 */
BYTE fixmode; /* 0 = none; 1 = 2D, 2 = 3D; 3 = 3D+DGPS */
BYTE svnum; /* number of sats in fix */
WORD gps_week; /* GPS Week Number */
DWORD gps_tow; /* Time of week, seconds, 1/100th resolution */
int lattitde; /* 1/1e-7 (???) degree, positive north */
int longitude; /* 1/1e-7 (???) degree, positive east */
DWORD elips_alt; /* meters, 1/100th */
DWORD sea_alt; /* meters, 1/100th */
WORD gdop; /* 1/100th */
WORD pdop; /* 1/100th */
WORD hdop; /* 1/100th */
WORD vdop; /* 1/100th */
WORD tdop; /* 1/100th */
int ecef_x; /* meters, 1/100th */
int ecef_y; /* meters, 1/100th */
int ecef_z; /* meters, 1/100th */
int ecef_vx; /* meters, 1/100th */
int ecef_vy; /* meters, 1/100th */
int ecef_vz; /* meters, 1/100th */
BYTE pad;
BYTE crlf[2]; /* CRLF at the end */
}
Note that it gives you ecef_vx,vy,vz With a simple transformation this can be turned into NED (Norht east down) velocities, exactly what you need to do the correction described in this thread. My guess is the lack of vertical velocity info on the Mediatek has prevented this from being implemented in the existing APM code base. Doing two derivatives of vertical position to yield vertical velocity is unlikely to work. Gps reported vertical velocity is and should be doppler derived and vertical position is not.
To transform the given ecf velocities to NED velocities I use the following code:
double ecefv[3];
double ned[3];
ecefv[0]=(int)(Last_Gps.ecef_vx);
ecefv[1]=(int)(Last_Gps.ecef_vy);
ecefv[2]=(int)(Last_Gps.ecef_vz);
ecefv[0]/=100.0;
ecefv[1]/=100.0;
ecefv[2]/=100.0;
ConvertVeolcity(ecefv,ned,cen);
where cen was setup with :
static double cen[3][3];
E2NDCM (lat,lon, cen);
The code for the supporting functions are:
// Given a current lat/lon, compute the direction cosine matrix cen[3][3]. Then, to
// compute the updated velocity N, E, D from either current X,Y,Z or velocity(X,Y or Z),
// use the matrix as follows:
// Velocity(North) = cen[0][0] * velocity(x) +
// cen[0][1] * velocity(y) +
// cen[0][2] * velocity(z);
// Velocity(East) = cen[1][0] * velocity(x) +
// cen[1][1] * velocity(y) +
// cen[1][2] * velocity(z);
// Velocity(Down) = cen[2][0] * velocity(x) +
// cen[2][1] * velocity(y) +
// cen[2][2] * velocity(z);
void E2NDCM (double lat, double lon, double cen[][3])
{ //
//
// Row CEN[0][i] is the North Unit vector in ECEF
// Row CEN[1][i] is the East Unit vector
// Row CEN[2][i] is the Down Unit vector
//
//
double slat, clat, slon, clon;
slat = sin (lat);
clat = cos (lat);
slon = sin (lon);
clon = cos (lon);
cen[0][0] = clon* slat;
cen[0][1] = slon* slat;
cen[0][2] = clat;
cen[1][0] = slon;
cen[1][1] = clon;
cen[1][2] = 0.0;
cen[2][0] = clon* clat;
cen[2][1] = slon* clat;
cen[2][2] = slat;
return;
} // end E2NDCM ()
void ConvertVeolcity(double ecefv[3],double ned[3], double cen[][3])
{
// Velocity(North) = cen[0][0] * velocity(x) +
// cen[0][1] * velocity(y) +
// cen[0][2] * velocity(z);
ned[0]=cen[0][0]*ecefv[0]+cen[0][1]*ecefv[1]+cen[0][2]*ecefv[2];
// Velocity(East) = cen[1][0] * velocity(x) +
// cen[1][1] * velocity(y) +
// cen[1][2] * velocity(z);
ned[1]=cen[1][0]*ecefv[0]+cen[1][1]*ecefv[1]+cen[1][2]*ecefv[2];
// Velocity(Down) = cen[2][0] * velocity(x) +
// cen[2][1] * velocity(y) +
// cen[2][2] * velocity(z);
ned[2]=cen[2][0]*ecefv[0]+cen[2][1]*ecefv[1]+cen[2][2]*ecefv[2];
}
This conversion code is derived from some geodetic code I found on google code... alas I've lost the reference and there was no copyright or other identifying info in the code itself...
I'm using this code for the IMU used in this video:
https://www.youtube.com/watch?v=CXEeCM13SdA&list=UUZI0BwkiC2Vm9V...
William, Andrew, just wondering what the status of this work is? It sounded really promising but has been quiet for almost two months now and it doesn't look like Tridge has made any updates to his branch in a long time. Has there been any progress?
Hi Bill,
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!
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 :-)
Cheers, Tridge
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 :-)
Angelo
Hi Bill,
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 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!
Cheers, Tridge
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.
Cheers, Tridge
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.
Cheers, Tridge