zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF: handle conversion of AHRS to handle altitude 

fixed accuracy of position for cm level lat/lng
mission-4.1.18
Andrew Tridgell 11 years ago
parent
804c43216b
commit
91cbad52a1
2 changed files with 78 additions and 143 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 192
      libraries/AP_NavEKF/AP_NavEKF.cpp
  2. 29
      libraries/AP_NavEKF/AP_NavEKF.h

192
libraries/AP_NavEKF/AP_NavEKF.cpp
Unescape View File

@ -86,18 +86,19 @@ bool NavEKF::healthy(void) @@ -86,18 +86,19 @@ bool NavEKF::healthy(void)
void NavEKF::InitialiseFilter(void)
{
lastFixTime = 0;
lastMagUpdate = 0;
lastAirspeedUpdate = 0;
// Calculate initial filter quaternion states from ahrs solution
Quaternion initQuat;
ahrsEul[0] = _ahrs->roll;
ahrsEul[1] = _ahrs->pitch;
ahrsEul[2] = _ahrs->yaw;
eul2quat(initQuat, ahrsEul);
initQuat.from_euler(_ahrs->roll, _ahrs->pitch, _ahrs->yaw);
// Calculate initial Tbn matrix and rotate Mag measurements into NED
// to set initial NED magnetic field states
Matrix3f DCM;
quat2Tbn(DCM, initQuat);
initQuat.rotation_matrix(DCM);
Vector3f initMagNED;
Vector3f initMagXYZ;
@ -115,15 +116,8 @@ void NavEKF::InitialiseFilter(void) @@ -115,15 +116,8 @@ void NavEKF::InitialiseFilter(void)
// read the barometer
readHgtData();
// reset reference position only if on-ground to allow for in-air restart
// set onground flag
OnGroundCheck();
if(onGround || !statesInitialised)
{
latRef = gpsLat;
lonRef = gpsLon;
hgtRef = _baro.get_altitude();
calcposNE(gpsLat, gpsLon);
}
// write to state vector
for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
@ -132,7 +126,7 @@ void NavEKF::InitialiseFilter(void) @@ -132,7 +126,7 @@ void NavEKF::InitialiseFilter(void)
states[6] = velNED[2];
states[7] = posNE[0];
states[8] = posNE[1];
states[9] = hgtRef - _baro.get_altitude();
states[9] = - _baro.get_altitude();
for (uint8_t j=0; j<=7; j++) states[j+10] = 0.0; // dAngBias, dVelBias, windVel
states[18] = initMagNED.x; // Magnetic Field North
states[19] = initMagNED.y; // Magnetic Field East
@ -145,7 +139,7 @@ void NavEKF::InitialiseFilter(void) @@ -145,7 +139,7 @@ void NavEKF::InitialiseFilter(void)
CovarianceInit();
//Define Earth rotation vector in the NED navigation frame
calcEarthRateNED(earthRateNED, latRef);
calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
//Initialise summed variables used by covariance prediction
summedDelAng.x = 0.0;
@ -158,49 +152,57 @@ void NavEKF::InitialiseFilter(void) @@ -158,49 +152,57 @@ void NavEKF::InitialiseFilter(void)
//Initialise IMU pre-processing states
readIMUData();
}
void NavEKF::UpdateFilter()
{
if (!statesInitialised) {
return;
}
perf_begin(_perf_UpdateFilter);
if (statesInitialised)
{
// This function will be called at 100Hz
//
// Read IMU data and convert to delta angles and velocities
readIMUData();
// Run the strapdown INS equations every IMU update
UpdateStrapdownEquationsNED();
// store the predicted states for subsequent use by measurement fusion
StoreStates();
// Check if on ground - status is used by covariance prediction
OnGroundCheck();
// sum delta angles and time used by covariance prediction
summedDelAng = summedDelAng + correctedDelAng;
summedDelVel = summedDelVel + correctedDelVel;
dt += dtIMU;
// perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update
// Do not predict covariance if magnetometer fusion still needs to be performed
if (((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)))
{
CovariancePrediction();
covPredStep = true;
summedDelAng.zero();
summedDelVel.zero();
dt = 0.0;
}
else
{
covPredStep = false;
}
// Update states using GPS, altimeter, compass and airspeed observations
SelectVelPosFusion();
SelectMagFusion();
SelectTasFusion();
// This function will be called at 100Hz
//
// Read IMU data and convert to delta angles and velocities
readIMUData();
if (dtIMU > 0.2f) {
// we have stalled for far too long - reset from DCM
InitialiseFilter();
perf_end(_perf_UpdateFilter);
return;
}
// Run the strapdown INS equations every IMU update
UpdateStrapdownEquationsNED();
// store the predicted states for subsequent use by measurement fusion
StoreStates();
// Check if on ground - status is used by covariance prediction
OnGroundCheck();
// sum delta angles and time used by covariance prediction
summedDelAng = summedDelAng + correctedDelAng;
summedDelVel = summedDelVel + correctedDelVel;
dt += dtIMU;
// perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update
// Do not predict covariance if magnetometer fusion still needs to be performed
if (((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax))) {
CovariancePrediction();
covPredStep = true;
summedDelAng.zero();
summedDelVel.zero();
dt = 0.0;
} else {
covPredStep = false;
}
// Update states using GPS, altimeter, compass and airspeed observations
SelectVelPosFusion();
SelectMagFusion();
SelectTasFusion();
perf_end(_perf_UpdateFilter);
}
@ -252,8 +254,8 @@ void NavEKF::SelectMagFusion() @@ -252,8 +254,8 @@ void NavEKF::SelectMagFusion()
{
readMagData();
// Fuse Magnetometer Measurements - hold off if pos/vel fusion has occurred, unless maximum time interval exceeded
bool dataReady = statesInitialised && useCompass && newDataMag;
bool timeout = ((IMUmsec - MAGmsecPrev) >= MAGmsecMax);
bool dataReady = statesInitialised && useCompass && newDataMag;
bool timeout = ((IMUmsec - MAGmsecPrev) >= MAGmsecMax);
if (dataReady && (!posVelFuseStep || timeout || fuseMeNow))
{
MAGmsecPrev = IMUmsec;
@ -1882,62 +1884,16 @@ void NavEKF::RecallStates(Vector24 &statesForFusion, uint32_t msec) @@ -1882,62 +1884,16 @@ void NavEKF::RecallStates(Vector24 &statesForFusion, uint32_t msec)
}
}
void NavEKF::quat2Tnb(Matrix3f &Tnb, const Quaternion &quat) const
{
// Calculate the nav to body cosine matrix
float q00 = sq(quat[0]);
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];
Tnb.a.x = q00 + q11 - q22 - q33;
Tnb.b.y = q00 - q11 + q22 - q33;
Tnb.c.z = q00 - q11 - q22 + q33;
Tnb.b.x = 2*(q12 - q03);
Tnb.c.x = 2*(q13 + q02);
Tnb.a.y = 2*(q12 + q03);
Tnb.c.y = 2*(q23 - q01);
Tnb.a.z = 2*(q13 - q02);
Tnb.b.z = 2*(q23 + q01);
}
void NavEKF::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const
{
// Calculate the body to nav cosine matrix
quat.rotation_matrix(Tbn);
}
void NavEKF::eul2quat(Quaternion &quat, const Vector3f &eul) const
{
float u1 = cosf(0.5f*eul[0]);
float u2 = cosf(0.5f*eul[1]);
float u3 = cosf(0.5f*eul[2]);
float u4 = sinf(0.5f*eul[0]);
float u5 = sinf(0.5f*eul[1]);
float u6 = sinf(0.5f*eul[2]);
quat[0] = u1*u2*u3+u4*u5*u6;
quat[1] = u4*u2*u3-u1*u5*u6;
quat[2] = u1*u5*u3+u4*u2*u6;
quat[3] = u1*u2*u6-u4*u5*u3;
}
void NavEKF::quat2eul(Vector3f &y, const Quaternion &u) const
{
y[0] = atan2f((2*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
y[1] = -asinf(2*(u[1]*u[3]-u[0]*u[2]));
y[2] = atan2f((2*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
}
void NavEKF::getEulerAngles(Vector3f &euler) const
{
Quaternion q(states[0], states[1], states[2], states[3]);
quat2eul(euler, q);
q.to_euler(&euler.x, &euler.y, &euler.z);
}
void NavEKF::getVelNED(Vector3f &vel) const
@ -1990,17 +1946,12 @@ void NavEKF::getMagXYZ(Vector3f &magXYZ) const @@ -1990,17 +1946,12 @@ void NavEKF::getMagXYZ(Vector3f &magXYZ) const
magXYZ.z = states[23]*1000.0f;
}
void NavEKF::calcposNE(float lat, float lon)
{
posNE[0] = RADIUS_OF_EARTH * (lat - latRef);
posNE[1] = RADIUS_OF_EARTH * cosf(latRef) * (lon - lonRef);
}
bool NavEKF::getLLH(struct Location &loc) const
{
loc.lat = 1.0e7f * degrees(latRef + states[7] / RADIUS_OF_EARTH);
loc.lng = 1.0e7f * degrees(lonRef + (states[8] / RADIUS_OF_EARTH) / cosf(latRef));
// loc.alt = 1.0e2f * (hgtRef - states[9]);
loc.lat = _ahrs->get_home().lat;
loc.lng = _ahrs->get_home().lng;
loc.alt = _ahrs->get_home().alt - states[9]*100;
location_offset(loc, states[7], states[8]);
return true;
}
@ -2011,7 +1962,7 @@ void NavEKF::OnGroundCheck() @@ -2011,7 +1962,7 @@ void NavEKF::OnGroundCheck()
uint8_t lowHgt;
lowAirSpd = (uint8_t)(_ahrs->airspeed_estimate_true(&VtasMeas) < 8.0f);
lowGndSpd = (uint8_t)((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f);
lowHgt = (uint8_t)(hgtMea < 15.0f);
lowHgt = (uint8_t)(fabsf(hgtMea < 15.0f));
// Go with a majority vote from three criteria
onGround = ((lowAirSpd + lowGndSpd + lowHgt) >= 2);
}
@ -2075,11 +2026,15 @@ void NavEKF::readGpsData() @@ -2075,11 +2026,15 @@ void NavEKF::readGpsData()
velNED[0] = _ahrs->get_gps()->velocity_north(); // (rad)
velNED[1] = _ahrs->get_gps()->velocity_east(); // (m/s)
velNED[2] = _ahrs->get_gps()->velocity_down(); // (m/s)
gpsLat = DEG_TO_RAD * 1e-7f * _ahrs->get_gps()->latitude; // (rad)
gpsLon = DEG_TO_RAD * 1e-7f * _ahrs->get_gps()->longitude; //(rad)
gpsHgt = 0.01f * _ahrs->get_gps()->altitude_cm; // (m)
// Convert GPS measurements to Pos NE
calcposNE(gpsLat, gpsLon);
struct Location gpsloc;
gpsloc.lat = _ahrs->get_gps()->latitude;
gpsloc.lng = _ahrs->get_gps()->longitude;
Vector2f posdiff = location_diff(_ahrs->get_home(), gpsloc);
posNE[0] = posdiff.x;
posNE[1] = posdiff.y;
}
}
@ -2087,7 +2042,7 @@ void NavEKF::readHgtData() @@ -2087,7 +2042,7 @@ void NavEKF::readHgtData()
{
// ToDo do we check for new height data or grab a height measurement?
// Best to do this at the same time as GPS measurement fusion for efficiency
hgtMea = _baro.get_altitude() - hgtRef;
hgtMea = _baro.get_altitude();
// recall states from compass measurement time
RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
}
@ -2123,11 +2078,12 @@ void NavEKF::readAirSpdData() @@ -2123,11 +2078,12 @@ void NavEKF::readAirSpdData()
}
}
void NavEKF::calcEarthRateNED(Vector3f &omega, float latitude) const
void NavEKF::calcEarthRateNED(Vector3f &omega, int32_t latitude) const
{
omega.x = earthRate*cosf(latitude);
float lat_rad = radians(latitude*1.0e-7f);
omega.x = earthRate*cosf(lat_rad);
omega.y = 0;
omega.z = -earthRate*sinf(latitude);
omega.z = -earthRate*sinf(lat_rad);
}
void NavEKF::getRotationBodyToNED(Matrix3f &mat) const

29
libraries/AP_NavEKF/AP_NavEKF.h
Unescape View File

@ -82,6 +82,9 @@ public: @@ -82,6 +82,9 @@ public:
// fill in latitude, longitude and height of the reference point
void getRefLLH(struct Location &loc) const;
// set latitude, longitude and height of the reference point
void setRefLLH(int32_t lat, int32_t lng, int32_t alt_cm);
// return the last calculated NED position relative to the
// reference point (m). Return false if no position is available
bool getPosNED(Vector3f &pos) const;
@ -145,20 +148,12 @@ private: @@ -145,20 +148,12 @@ private:
// recall state vector stored at closest time to the one specified by msec
void RecallStates(Vector24 &statesForFusion, uint32_t msec);
void quat2Tnb(Matrix3f &Tnb, const Quaternion &quat) const;
void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const;
void calcEarthRateNED(Vector3f &omega, float latitude) const;
void eul2quat(Quaternion &quat, const Vector3f &eul) const;
void quat2eul(Vector3f &eul, const Quaternion &quat) const;
void calcEarthRateNED(Vector3f &omega, int32_t latitude) const;
void calcvelNED(Vector3f &velNED, float gpsCourse, float gpsGndSpd, float gpsVelD) const;
void calcposNE(float lat, float lon);
void calcllh(float &lat, float &lon, float &hgt) const;
void OnGroundCheck();
@ -243,12 +238,7 @@ private: @@ -243,12 +238,7 @@ private:
bool fuseVtasData; // boolean true when airspeed data is to be fused
float VtasMeas; // true airspeed measurement (m/s)
Vector24 statesAtVtasMeasTime; // filter states at the effective measurement time
float latRef; // WGS-84 latitude of reference point (rad)
float lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
Vector3f magBias; // magnetometer bias vector in XYZ body axes
Vector3f eulerEst; // Euler angles calculated from filter states
Vector3f eulerDif; // difference between Euler angle estimated by EKF and the AHRS solution
const float covTimeStepMax; // maximum time allowed between covariance predictions
const float covDelAngMax; // maximum delta angle between covariance predictions
bool covPredStep; // boolean set to true when a covariance prediction step has been performed
@ -288,26 +278,15 @@ private: @@ -288,26 +278,15 @@ private:
// GPS input data variables
float gpsCourse;
float gpsGndSpd;
float gpsLat;
float gpsLon;
float gpsHgt;
bool newDataGps;
// Magnetometer input data variables
float magIn;
Vector8 tempMag;
Vector8 tempMagPrev;
uint32_t MAGframe;
uint32_t MAGtime;
uint32_t lastMAGtime;
bool newDataMag;
// TAS input variables
bool newDataTas;
// AHRS input data variables
Vector3f ahrsEul;
// Time stamp when vel, pos or height measurements last failed checks
uint32_t velFailTime;
uint32_t posFailTime;

Write Preview
Loading…
Cancel
Save