An improved method for accounting for acceleration in gyro roll-pitch drift computations

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

Views: 12035

Replies to This Discussion

Hi Bill,

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

I did try that something along those lines. What I did was to keep a drift_error_earth vector, which gets its X and Y components from the accel drift correction (the R*A sum). It gets its Z component solely from the yaw drift correction. That vector is then converted to body frame to feed into the PI controller. That means the R*A sum is (in earth frame) the only contributor to roll/pitch correction, and the yaw drift is the only contributor to the Z correction.

I was hoping this separation would help prevent cross-talk between the two drift correction systems building up with sustained turns. Unfortunately it didn't work, and didn't even really make a difference at all, which suggests I'm on the wrong track.

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.

I do zero the R*A sum at each GPS fix. I had a bug at one stage where I didn't do that, and it collapsed a lot more quickly than 10 minutes! I think it managed to fly for about 30s or so :-)

One thing I'm going to try soon is accumulating omegaI changes over much longer time periods. Right now omegaI is adjusted on every GPS fix, which means at 4Hz. We know that what we want omegaI to do is follow the long term drift of the gyros, which means we want it to change on much longer timescales than 0.25 seconds. So I'm thinking of accumulating (but not applying) the omegaI change over a period of 10 to 15 seconds, and then applying it as a step, but limiting the step size to the known maximum drift rate of the gyros. This system would rely on omegaP to handle the small amount of drift that can build up between steps.

The idea is to prevent omegaI from trying to track anything that is happening over the short term, for example in turns, only allowing it to track the long term trend. If we integrate over a long enough time then omegaI has got to come out right. My guess is that 10s to 15s will be about right, but I'll try a range of integration periods and see what works.

I'll try that idea first, and then move onto more detailed graphs of what is happening to omegaP.

Cheers, Tridge

Hi Bill,

In your sustained rotations report, you were dealing with a rotation rate of over 400 degrees/s. The tests I'm doing at the moment are flying circuits with a average yaw rate of under 5 degrees/s, and the long term average roll and pitch rate is even lower. I hadn't paid a lot of attention to the analysis from your report as I expected that sustained rotation rates that low wouldn't cause the sort of error build up that you were looking at.

I also have not studied Magoney's original proof, and I should do that.

Cheers, Tridge

Hi Tridge,

Thanks. The fact that the algorithm passed the "bad attitude" tests is tantalizing, it makes me think that you and I can get this working.

I wonder if you would be willing to run another test case for me:

Simulate a motionless aircraft sitting on the ground, for 20 minutes, or until the algorithm goes unstable.

The reason for that is I would like to find out if that test case is a "litmus" test I can use on my end. What I would like to do is implement the algorithm on my end in our "roll-pitch-yaw demo" and do ground testing with a UDB just sitting on my desk.

Best regards,

Bill

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 R_Lefebvre,

This technique has been implemented, tested, and released on May 3 in the repository for MatrixPilot. Some of the flight testing is described here. Since then, there has been extensive testing with fixed wing, with good results, including some fixed wing flights with magnetometer. Presently, Mark Whitehorn is developing "MatrixPilotQuad", which will include this technique.

I have not talked to Tridge lately, so I do not know what progress he has made. I do note that he was working with quads, and that so far, the MatrixPilot team has focused on fixed wing. Its my impression that Tridge thought there might be an issue related to cross coupling of error effects when a magnetometer is involved.

I did ground testing with magnetometer and the new method, that worked fine. I have not personally done flight testing with magnetometer and the new method.

One of the things on my "to do" list is to work with Mark Whitehorn to flight test this technique on a quad. Mark has made quite a bit of progress, and has made a number of flights, including flights in which he pushed some limits. For example, in one of Mark's flights, he spun his quad around its yaw axis at 250 degrees per second, while flying it around in "simple" control mode. But I have not looked closely at his flight data yet.

Best regards,

Bill Premerlani

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

Hi R_Lefebvre,

In order to increase my confidence in this new method for acceleration compensation, I did some more flight testing today with a magnetometer included in the system, to assure that cross coupling between magnetometer yaw error and acceleration roll-pitch error would not cause problems. I flew r1546 of MatrixPilot trunk on an EasyStar for 30 minutes, all the while trying to break the algorithm by sustained high acceleration turns. GPS is an EM406. Performance was great. Attached are a couple of photos of the flight track.

The first photo shows a 5 or 6 rotation turn followed by releasing the stick to the center. Note that the plane quickly straightened out, indicating no residual roll error. Then, with the plane flying away from me, I switched into return to launch mode, at which point the plane did a 180 degree turn and headed straight toward me. Also note that the estimated yaw orientation of the plane nicely matches the direction of travel. This means that the magnetometer yaw error was small.

The second photo shows the complete flight. Most of it was done in manual mode, mostly sustained turns, both clockwise and counter clockwise. For a few of the turns, I applied full rudder. A few times I commanded RTL in the course of a turn, the plane always turned and flew straight toward me.

Best regards,

Bill

Hi William,

I've read your document about DCM Draft 2, as mentioned at the end, where can I find the document relative to the implementation in C of the DCM matrix?

Many thanks.

Kind regards

Bernard

Hi Bernard,

There used to be an implementation document, but I got tired of updating it each time we revised the implementation, so it no longer exists. There have been a lot of improvements and extensions to the basic idea. For some of the improvements, take a look at the UDB page. For implementation, take a look at the code.

Best regards,

Bill

Hello William,

I’m trying to locate the code (within the APM, Arduplane or Arducopter files) that a) initiates and b) performs, the compass calibration routine but I had no luck so far. Can you please provide some insight?

Regards

Evan

Hi Evan,

I use the UAVDevboard, and I am not familiar with the Ardu firmware, I suggest you repost your question to diydrones as a discussion.

Best regards,

Bill Premerlani

Hi Evan,

Have a look in libraries/AP_Compass/Compass.cpp in null_offsets()

The compass calibration is continuous if COMPASS_LEARN is enabled. See the call to null_offsets() in ArduPlane.pde

Cheers, Tridge

1

2

3

4

5

6

7

8

9

10

Groups

631 members

259 members

281 members

1470 members

• ArduCopter User Group

3164 members

Season Two of the Trust Time Trial (T3) Contest
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here