Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: first working GPS fusion

Browse Source
This commit is contained in:
Paul Riseborough 2013-12-30 14:43:29 +11:00 committed by Andrew Tridgell
parent 90cd04def3
commit bccadb6e25
1 changed files with 23 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -181,7 +181,7 @@ void NavEKF::UpdateFilter()
} }
// Update states using GPS, altimeter, compass and airspeed observations // Update states using GPS, altimeter, compass and airspeed observations
//SelectVelPosFusion(); SelectVelPosFusion();
//SelectMagFusion(); //SelectMagFusion();
//SelectTasFusion(); //SelectTasFusion();
} }
@ -260,16 +260,6 @@ void NavEKF::SelectTasFusion()
void NavEKF::UpdateStrapdownEquationsNED() void NavEKF::UpdateStrapdownEquationsNED()
{ {
Vector3f delVelNav; Vector3f delVelNav;
float q00;
float q11;
float q22;
float q33;
float q01;
float q02;
float q03;
float q12;
float q13;
float q23;
float rotationMag; float rotationMag;
float rotScaler; float rotScaler;
Quaternion qUpdated; Quaternion qUpdated;
@ -330,38 +320,16 @@ void NavEKF::UpdateStrapdownEquationsNED()
} }
// Calculate the body to nav cosine matrix // Calculate the body to nav cosine matrix
q00 = sq(states[0]); Quaternion q(states[0],states[1],states[2],states[3]);
q11 = sq(states[1]); Matrix3f Tbn_temp;
q22 = sq(states[2]); q.rotation_matrix(Tbn_temp);
q33 = sq(states[3]); prevTnb = Tbn_temp.transposed();
q01 = states[0]*states[1];
q02 = states[0]*states[2];
q03 = states[0]*states[3];
q12 = states[1]*states[2];
q13 = states[1]*states[3];
q23 = states[2]*states[3];
Matrix3f Tbn;
Tbn.a.x = q00 + q11 - q22 - q33;
Tbn.b.y = q00 - q11 + q22 - q33;
Tbn.c.z = q00 - q11 - q22 + q33;
Tbn.a.y = 2*(q12 - q03);
Tbn.a.z = 2*(q13 + q02);
Tbn.b.x = 2*(q12 + q03);
Tbn.b.z = 2*(q23 - q01);
Tbn.c.x = 2*(q13 - q02);
Tbn.c.y = 2*(q23 + q01);
prevTnb = Tbn.transpose();
// transform body delta velocities to delta velocities in the nav frame // transform body delta velocities to delta velocities in the nav frame
// * and + operators have been overloaded // * and + operators have been overloaded
//delVelNav = Tbn*correctedDelVel + gravityNED*dtIMU; delVelNav = Tbn_temp*correctedDelVel + gravityNED*dtIMU;
delVelNav.x = Tbn.a.x*correctedDelVel.x + Tbn.a.y*correctedDelVel.y + Tbn.a.z*correctedDelVel.z + gravityNED.x*dtIMU;
delVelNav.y = Tbn.b.x*correctedDelVel.x + Tbn.b.y*correctedDelVel.y + Tbn.b.z*correctedDelVel.z + gravityNED.y*dtIMU;
delVelNav.z = Tbn.c.x*correctedDelVel.x + Tbn.c.y*correctedDelVel.y + Tbn.c.z*correctedDelVel.z + gravityNED.z*dtIMU;
::printf(" accNav: (%.4f %.4f %.4f) \n", delVelNav.x/dtIMU,delVelNav.y/dtIMU,delVelNav.z/dtIMU); //::printf(" accNav: (%.4f %.4f %.4f) \n", delVelNav.x/dtIMU,delVelNav.y/dtIMU,delVelNav.z/dtIMU);
// calculate the magnitude of the nav acceleration (required for GPS // calculate the magnitude of the nav acceleration (required for GPS
// variance estimation) // variance estimation)
accNavMag = delVelNav.length()/dtIMU; accNavMag = delVelNav.length()/dtIMU;
@ -1220,8 +1188,9 @@ void NavEKF::FuseVelPosNED()
// Form the observation vector // Form the observation vector
for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i];
for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3];
observation[5] = -(hgtMea); observation[5] = -hgtMea;
::printf("observation = (%.1f, %.1f, %.1f, %.1f, %.1f, %.1f \n",
observation[0],observation[1],observation[2],observation[3],observation[4],observation[5]);
// Estimate the GPS Velocity, GPS horiz position and height measurement variances. // Estimate the GPS Velocity, GPS horiz position and height measurement variances.
velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
@ -1242,7 +1211,7 @@ void NavEKF::FuseVelPosNED()
if (fusionModeGPS == 1) imax = 1; if (fusionModeGPS == 1) imax = 1;
for (uint8_t i = 0; i<=imax; i++) for (uint8_t i = 0; i<=imax; i++)
{ {
velInnov[i] = statesAtVelTime[i+4] - velNED[i]; velInnov[i] = statesAtVelTime[i+4] - observation[i];
stateIndex = 4 + i; stateIndex = 4 + i;
varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
} }
@ -1263,8 +1232,8 @@ void NavEKF::FuseVelPosNED()
if (fusePosData) if (fusePosData)
{ {
// test horizontal position measurements // test horizontal position measurements
posInnov[0] = statesAtPosTime[7] - posNE[0]; posInnov[0] = statesAtPosTime[7] - observation[3];
posInnov[1] = statesAtPosTime[8] - posNE[1]; posInnov[1] = statesAtPosTime[8] - observation[4];
varInnovVelPos[3] = P[7][7] + R_OBS[3]; varInnovVelPos[3] = P[7][7] + R_OBS[3];
varInnovVelPos[4] = P[8][8] + R_OBS[4]; varInnovVelPos[4] = P[8][8] + R_OBS[4];
// apply a 10-sigma threshold // apply a 10-sigma threshold
@ -1283,7 +1252,7 @@ void NavEKF::FuseVelPosNED()
// test height measurements // test height measurements
if (fuseHgtData) if (fuseHgtData)
{ {
hgtInnov = statesAtHgtTime[9] + hgtMea; hgtInnov = statesAtHgtTime[9] - observation[5];
varInnovVelPos[5] = P[9][9] + R_OBS[5]; varInnovVelPos[5] = P[9][9] + R_OBS[5];
// apply a 10-sigma threshold // apply a 10-sigma threshold
//debug //debug
@ -1354,13 +1323,13 @@ void NavEKF::FuseVelPosNED()
if (obsIndex == 5) if (obsIndex == 5)
{ {
::printf("index = %.1f, obs = %.1f, pred = %.1f, innov = %.2f, variance = %.4f\n",float(obsIndex),observation[obsIndex],statesAtHgtTime[stateIndex],innovVelPos[obsIndex],varInnovVelPos[obsIndex]);
::printf("K_fusion = "); ::printf("K_fusion = ");
for (uint8_t i= 0; i<=23; i++) for (uint8_t i= 0; i<=23; i++)
{ {
::printf("%.2f, ",Kfusion[i]); ::printf("%.2f, ",Kfusion[i]);
} }
::printf("\n"); ::printf("\n");
::printf("index = %.1f, innov = %.2f, variance = %.4f\n",float(obsIndex),innovVelPos[obsIndex],varInnovVelPos[obsIndex]);
} }
// Calculate state corrections and re-normalise the quaternions // Calculate state corrections and re-normalise the quaternions
@ -1891,12 +1860,12 @@ void NavEKF::quat2Tnb(Matrix3f &Tnb, const Quaternion &quat)
float q11 = sq(quat[1]); float q11 = sq(quat[1]);
float q22 = sq(quat[2]); float q22 = sq(quat[2]);
float q33 = sq(quat[3]); float q33 = sq(quat[3]);
float q01 = quat[0]*quat[1]; float q01 = quat[0]*quat[1];
float q02 = quat[0]*quat[2]; float q02 = quat[0]*quat[2];
float q03 = quat[0]*quat[3]; float q03 = quat[0]*quat[3];
float q12 = quat[1]*quat[2]; float q12 = quat[1]*quat[2];
float q13 = quat[1]*quat[3]; float q13 = quat[1]*quat[3];
float q23 = quat[2]*quat[3]; float q23 = quat[2]*quat[3];
Tnb.a.x = q00 + q11 - q22 - q33; Tnb.a.x = q00 + q11 - q22 - q33;
Tnb.b.y = q00 - q11 + q22 - q33; Tnb.b.y = q00 - q11 + q22 - q33;
@ -1912,26 +1881,7 @@ void NavEKF::quat2Tnb(Matrix3f &Tnb, const Quaternion &quat)
void NavEKF::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) void NavEKF::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat)
{ {
// Calculate the body to nav cosine matrix // Calculate the body to nav cosine matrix
float q00 = sq(quat[0]); quat.rotation_matrix(Tbn);
float q11 = sq(quat[1]);
float q22 = sq(quat[2]);
float q33 = sq(quat[3]);
float q01 = quat[0]*quat[1];
float q02 = quat[0]*quat[2];
float q03 = quat[0]*quat[3];
float q12 = quat[1]*quat[2];
float q13 = quat[1]*quat[3];
float q23 = quat[2]*quat[3];
Tbn.a.x = q00 + q11 - q22 - q33;
Tbn.b.y = q00 - q11 + q22 - q33;
Tbn.c.z = q00 - q11 - q22 + q33;
Tbn.a.y = 2*(q12 - q03);
Tbn.a.z = 2*(q13 + q02);
Tbn.b.x = 2*(q12 + q03);
Tbn.b.z = 2*(q23 - q01);
Tbn.c.x = 2*(q13 - q02);
Tbn.c.y = 2*(q23 + q01);
} }
void NavEKF::eul2quat(Quaternion &quat, const Vector3f &eul) void NavEKF::eul2quat(Quaternion &quat, const Vector3f &eul)
@ -2054,7 +2004,7 @@ void NavEKF::readHgtData()
// Best to do this at the same time as GPS measurement fusion for efficiency // Best to do this at the same time as GPS measurement fusion for efficiency
hgtMea = _baro.get_altitude() - hgtRef; hgtMea = _baro.get_altitude() - hgtRef;
// recall states from compass measurement time // recall states from compass measurement time
RecallStates(statesAtVelTime, (IMUmsec - msecHgtDelay)); RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
} }
void NavEKF::readMagData() void NavEKF::readMagData()