# 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

Tags: centrifugal

Views: 6094

### Replies to This Discussion

Hi Tridge,

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.

Best regards,

Bill

Hi Bill,

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.

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

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

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

26 members

1800 members

86 members

83 members

• ### Arducopter Y6 Owners

97 members

Season Two of the Trust Time Trial (T3) Contest has now begun. The fifth round, 3D model building requiring contestants to survey and reconstruct a building or natural scene from aerial images.

A list of all T3 contests is here.