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
Brillant job. Thanks a lot William.
I went through the code in file AP_AHRS_DCM.cpp, it seems ArduCopter(Quadcopter) runs only this algorithm on APM2 for gyro drift compensation.
But it relys on GPS and my APM2 flys perfectly indoors (with GPS intalled, having not location fixed).
Anyone has idea? Is there anyother gyro drift compensation algorithm (without help of GPS) in the ArduCopter?
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.
Thanks for reply, Philip.
I went through the function drift_correction function in class AP_AHRS_DCM. But I could not find any logic in the code that will redirect to other accelerometer based roll-pitch compensation when GPS is not available(yaw compensation based on magnetometer and GPS is clear for me now).
For APM2 based ArduCopter firmware, I think ahrs in ArduCopter.ino is a object of class AP_AHRS_DCM, while ins in ArduCopter.ino is a object of class AP_InertialSensor_MPU600. This understanding should be right for 3.01. and 3.1.5 version.
Would you please tell me more details about how the code shifts to accelerometer based roll-pitch compensation? if you are familiar with the ArduCopter code.
Thanks in advance.
Best regards.
Yi
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.
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
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
-
1
-
2
of 2 Next