Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 10:38:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Updated measurement fusion control

Browse Source
This commit is contained in:
Paul Riseborough 2013-12-28 20:24:01 +11:00 committed by Andrew Tridgell
parent c557bd7df5
commit 0f3ebb8e31
2 changed files with 102 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

127
libraries/AP_NavEKF/NavEKF.cpp
View File

@ -12,21 +12,28 @@ void InitialiseFilter()
// to set initial NED magnetic field states // to set initial NED magnetic field states
Mat3f DCM; Mat3f DCM;
quat2Tbn(DCM, initQuat); quat2Tbn(DCM, initQuat);
Vector3f initMagNED; Vector3f initMagNED = {0,0,0};
Vector3f initMagXYZ; Vector3f initMagXYZ = {0,0,0};
if (useCompass)
{
readMagData();
initMagXYZ.x = magData - magBias; initMagXYZ.x = magData - magBias;
initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z;
initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
}
// calculate initial velocities // calculate initial velocities
float initvelNED[3]; float initvelNED[3];
calcvelNED(initvelNED, gpsCourse, gpsGndSpd, gpsVelD); calcvelNED(initvelNED, gpsCourse, gpsGndSpd, gpsVelD);
//store initial lat,long and height // reset reference position only if on-ground to allow for in-air restart
if(onGround)
{
latRef = gpsLat; latRef = gpsLat;
lonRef = gpsLon; lonRef = gpsLon
hgtRef = barometer.get_altitude(); hgtRef = barometer.get_altitude();
}
// write to state vector // write to state vector
for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
@ -60,6 +67,8 @@ void UpdateFilter()
{ {
if (statesInitialised) if (statesInitialised)
{ {
// This function will be called at 100Hz
//
// Read IMU data and convert to delta angles and velocities // Read IMU data and convert to delta angles and velocities
readIMUdata(); readIMUdata();
// Run the strapdown INS equations every IMU update // Run the strapdown INS equations every IMU update
@ -74,89 +83,87 @@ void UpdateFilter()
dt += dtIMU; dt += dtIMU;
// perform a covariance prediction if the total delta angle has exceeded the limit // 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 // or the time limit will be exceeded at the next IMU update
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) // Do not predict covariance if magnetometer fusion still needs to be performed
if (((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) && !magFuseStep)
{ {
CovariancePrediction(); CovariancePrediction();
covPredStep = true;
summedDelAng = summedDelAng.zero(); summedDelAng = summedDelAng.zero();
summedDelVel = summedDelVel.zero(); summedDelVel = summedDelVel.zero();
dt = 0.0; dt = 0.0;
} }
// Check for new Magnetometer Data else
readMagData(); {
// Check for new GPS Data covPredStep = false;
readGpsData(); }
// Check for new height Data
readHgtData(); // Update states using GPS, altimeter, compass and airspeed observations
// Check for new TAS data SelectVelPosFusion();
readTasData(); SelectMagFusion();
SelectTasFusion();
} }
} }
void FuseGPS() void SelectVelPosFusion()
{ {
// Fuse GPS Measurements // Command fusion of GPS measurements if new ones available
if (statesInitialised) readGpsData();
if (statesInitialised && newDataGps)
{ {
// set fusion flags
fuseVelData = true; fuseVelData = true;
fusePosData = true; fusePosData = true;
// run the fusion step
FuseVelposNED();
fuseVelData = false;
fusePosData = false;
} }
else else
{ {
fuseVelData = false; fuseVelData = false;
fusePosData = false; fusePosData = false;
} }
} // Fuse height measurements with GPS measurements for efficiency
// Don't wait longer than HGTmsecTgt msec between height fusion steps
void FuseHGT() if (statesInitialised && (newDataGps || ((IMUmsec - HGTmsecPrev) >= HGTmsecTgt)))
{
// Fuse GPS Measurements
if (statesInitialised)
{ {
// set fusion flags
fuseHgtData = true; fuseHgtData = true;
// run the fusion step readHgtData();
FuseVelposNED();
fuseHgtData = false;
} }
else else
{ {
fuseHgtData = false; fuseHgtData = false;
} }
FuseVelposNED();
fuseHgtData = false;
} }
void FuseMAG() void SelectMagFusion()
{ {
// Fuse Magnetometer Measurements // Fuse Magnetometer Measurements
if (statesInitialised) if (statesInitialised && useCompass && !covPredStep && ((IMUmsec - MAGmsecPrev) >= MAGmsecTgt))
{ {
MAGmsecPrev = IMUmsec;
fuseMagData = true; fuseMagData = true;
FuseMagnetometer(); readMagData();
fuseMagData = false;
} }
else else
{ {
fuseMagData = false; fuseMagData = false;
// protect against wrap-around
if(IMUmsec < MAGmsecPrev) MAGmsecPrev = IMUmsec;
} }
// Magnetometer fusion is always called if enabled because its fusion is spread across 3 time steps
// to reduce peak load
FuseMagnetometer();
} }
void FuseTAS() void SelectTasFusion()
{ {
// Fuse Airspeed Measurements // Fuse Airspeed Measurements
if (statesInitialised && VtasMeas > 8.0) if (statesInitialised && useAirspeed && !onGround && !covPredStep && !magFuseStep && ((IMUmsec - TASmsecPrev) >= TASmsecTgt))
{ {
fuseVtasData = true; TASmsecPrev = IMUmsec;
readAirSpdData();
FuseAirspeed(); FuseAirspeed();
fuseVtasData = false;
}
else
{
fuseVtasData = false;
} }
// protect against wrap-around
if(IMUmsec < TASmsecPrev) TASmsecPrev = IMUmsec;
} }
void UpdateStrapdownEquationsNED() void UpdateStrapdownEquationsNED()
@ -1321,12 +1328,10 @@ void FuseMagnetometer()
// data fit is the only assumption we can make // data fit is the only assumption we can make
// so we might as well take advantage of the computational efficiencies // so we might as well take advantage of the computational efficiencies
// associated with sequential fusion // associated with sequential fusion
if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) if (fuseMagData || obsIndex == 1 || obsIndex == 2)
{ {
// Sequential fusion of XYZ components to spread processing load across // Sequential fusion of XYZ components to spread processing load across
// three prediction time steps. // three prediction time steps.
// Calculate observation jacobians and Kalman gains // Calculate observation jacobians and Kalman gains
if (fuseMagData) if (fuseMagData)
{ {
@ -1414,10 +1419,11 @@ void FuseMagnetometer()
Kfusion[22] = SK_MX[0]*(P[22][21] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][18]*SK_MX[1] + P[22][19]*SK_MX[5] - P[22][20]*SK_MX[4]); Kfusion[22] = SK_MX[0]*(P[22][21] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][18]*SK_MX[1] + P[22][19]*SK_MX[5] - P[22][20]*SK_MX[4]);
Kfusion[23] = SK_MX[0]*(P[23][21] + P[23][1]*SH_MAG[0] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[3] - P[23][2]*SK_MX[2] - P[23][18]*SK_MX[1] + P[23][19]*SK_MX[5] - P[23][20]*SK_MX[4]); Kfusion[23] = SK_MX[0]*(P[23][21] + P[23][1]*SH_MAG[0] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[3] - P[23][2]*SK_MX[2] - P[23][18]*SK_MX[1] + P[23][19]*SK_MX[5] - P[23][20]*SK_MX[4]);
varInnovMag[0] = 1.0/SK_MX[0]; varInnovMag[0] = 1.0/SK_MX[0];
// reset the observation index to 0 (we start by fusing the X // reset the observation index to 0 (we start by fusing the X
// measurement) // measurement)
obsIndex = 0; obsIndex = 0;
// Set flag to indicate to other processes that magnetometer fusion is performed on this time step
magFuseStep = true;
} }
else if (obsIndex == 1) // we are now fusing the Y measurement else if (obsIndex == 1) // we are now fusing the Y measurement
{ {
@ -1465,6 +1471,8 @@ void FuseMagnetometer()
Kfusion[22] = SK_MY[0]*(P[22][22] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][19]*SK_MY[1] - P[22][18]*SK_MY[3] + P[22][20]*SK_MY[4]); Kfusion[22] = SK_MY[0]*(P[22][22] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][19]*SK_MY[1] - P[22][18]*SK_MY[3] + P[22][20]*SK_MY[4]);
Kfusion[23] = SK_MY[0]*(P[23][22] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][19]*SK_MY[1] - P[23][18]*SK_MY[3] + P[23][20]*SK_MY[4]); Kfusion[23] = SK_MY[0]*(P[23][22] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][19]*SK_MY[1] - P[23][18]*SK_MY[3] + P[23][20]*SK_MY[4]);
varInnovMag[1] = 1.0/SK_MY[0]; varInnovMag[1] = 1.0/SK_MY[0];
// Set flag to indicate to other processes that magnetometer fusion is performed on this time step
magFuseStep = true;
} }
else if (obsIndex == 2) // we are now fusing the Z measurement else if (obsIndex == 2) // we are now fusing the Z measurement
{ {
@ -1513,6 +1521,8 @@ void FuseMagnetometer()
Kfusion[22] = SK_MZ[0]*(P[22][23] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][20]*SK_MZ[1] + P[22][18]*SK_MZ[5] - P[22][19]*SK_MZ[4]); Kfusion[22] = SK_MZ[0]*(P[22][23] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][20]*SK_MZ[1] + P[22][18]*SK_MZ[5] - P[22][19]*SK_MZ[4]);
Kfusion[23] = SK_MZ[0]*(P[23][23] + P[23][0]*SH_MAG[1] + P[23][3]*SH_MAG[0] - P[23][1]*SK_MZ[2] + P[23][2]*SK_MZ[3] + P[23][20]*SK_MZ[1] + P[23][18]*SK_MZ[5] - P[23][19]*SK_MZ[4]); Kfusion[23] = SK_MZ[0]*(P[23][23] + P[23][0]*SH_MAG[1] + P[23][3]*SH_MAG[0] - P[23][1]*SK_MZ[2] + P[23][2]*SK_MZ[3] + P[23][20]*SK_MZ[1] + P[23][18]*SK_MZ[5] - P[23][19]*SK_MZ[4]);
varInnovMag[2] = 1.0/SK_MZ[0]; varInnovMag[2] = 1.0/SK_MZ[0];
// Set flag to indicate to other processes that magnetometer fusion is performed on this time step
magFuseStep = true;
} }
// Calculate the measurement innovation // Calculate the measurement innovation
innovMag[obsIndex] = MagPred[obsIndex] - magData[obsIndex]; innovMag[obsIndex] = MagPred[obsIndex] - magData[obsIndex];
@ -1574,6 +1584,11 @@ void FuseMagnetometer()
} }
obsIndex = obsIndex + 1; obsIndex = obsIndex + 1;
} }
else
{
// clear flag to indicate to other processes that magnetometer fusion is not performed on this time step
magFuseStep = false;
}
} }
void FuseAirspeed() void FuseAirspeed()
@ -1917,9 +1932,14 @@ void readIMUData()
void readGpsData() void readGpsData()
{ {
static uint64_t lastFixTime=0;
if ((g_gps->last_fix_time > g_gps->last_fix_time) && (g_gps->status() >= GPS::GPS_OK_FIX_3D))
{
GPSstatus = g_gps->status();
newDataGps = true;
RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay)); RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay));
RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay)); RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay));
GPSstatus = g_gps->status();
velNED[0] = g_gps->velocity_north(); // (rad) velNED[0] = g_gps->velocity_north(); // (rad)
velNED[1] = g_gps->velocity_east(); // (m/s) velNED[1] = g_gps->velocity_east(); // (m/s)
velNED[2] = g_gps->velocity_down(); // (m/s) velNED[2] = g_gps->velocity_down(); // (m/s)
@ -1928,19 +1948,24 @@ void readGpsData()
gpsHgt = 0.01*g_gps->altitude_cm(); // (m) gpsHgt = 0.01*g_gps->altitude_cm(); // (m)
// Convert GPS measurements to Pos NE // Convert GPS measurements to Pos NE
calcposNE(posNE, gpsLat, gpsLon, latRef, lonRef); calcposNE(posNE, gpsLat, gpsLon, latRef, lonRef);
}
} }
void readHgtData() void readHgtData()
{ {
// read barometric altitude in m // ToDo do we check for new height data or grab a height measurement?
hgtMea = barometer.get_altitude(); // Best to do this at the same time as GPS measurement fusion for efficiency
RecallStates(statesAtVelTime, (IMUmsec - msecHgtDelay)); hgtMea = barometer.get_altitude() - hgtRef;
// recall states from compass measurement time
if(newDataHgt) RecallStates(statesAtVelTime, (IMUmsec - msecHgtDelay));
} }
void readMagData() void readMagData()
{ {
// scale compass data to improve numerical conditioning
magData = 0.001*compass.get_field(); magData = 0.001*compass.get_field();
magBias = 0.001*compass.get_offsets(); magBias = 0.001*compass.get_offsets();
// Recall states from compass measurement time
RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay)); RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay));
} }

29
libraries/AP_NavEKF/NavEKF.h
View File

@ -111,13 +111,13 @@ void readMagData();
void readAirSpdData(); void readAirSpdData();
void FuseGPS(); void SelectVelPosFusion();
void FuseHGT(); void SelectHgtFusion();
void FuseTAS(); void SelectTasFusion();
void FuseMAG(); void SelectMagFusion();
#define GRAVITY_MSS 9.80665 #define GRAVITY_MSS 9.80665
#define deg2rad 0.017453292 #define deg2rad 0.017453292
@ -129,8 +129,8 @@ float KH[24][24]; // intermediate result used for covariance updates
float KHP[24][24]; // intermediate result used for covariance updates float KHP[24][24]; // intermediate result used for covariance updates
static float P[24][24]; // covariance matrix static float P[24][24]; // covariance matrix
float Kfusion[24]; // Kalman gains float Kfusion[24]; // Kalman gains
static float states[24]; // state matrix float states[24]; // state matrix
static float storedStates[24][50]; // state vectors stored for the last 50 time steps float storedStates[24][50]; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[50]; // time stamp for each state vector stored uint32_t statetimeStamp[50]; // time stamp for each state vector stored
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
@ -176,8 +176,16 @@ float eulerEst[3]; // Euler angles calculated from filter states
float eulerDif[3]; // difference between Euler angle estimated by EKF and the AHRS solution float eulerDif[3]; // difference between Euler angle estimated by EKF and the AHRS solution
const float covTimeStepMax = 0.07; // maximum time allowed between covariance predictions const float covTimeStepMax = 0.07; // maximum time allowed between covariance predictions
const float covDelAngMax = 0.05; // maximum delta angle between covariance predictions const float covDelAngMax = 0.05; // maximum delta angle between covariance predictions
bool endOfData = false; //boolean set to true when all files have returned data bool covPredStep = false; // boolean set to true when a covariance prediction step has been performed
bool magFuseStep = false; // boolean set to true when magnetometer fusion steps are being performed
bool posVelFuseStep = false; // boolean set to true when position and velocity fusion is being performed
bool tasFuseStep = false; // boolean set to true when airspeed fusion is being performed
uint32_t TASmsecPrev = 0; // time stamp of last TAS fusion step
uint32_t TASmsecTgt = 250; // target interval between TAS fusion steps
uint32_t TASmsecPrev = 0; // time stamp of last compass fusion step
uint32_t TASmsecTgt = 200; // target interval between compass fusion steps
uint32_t HGTmsecPrev = 0; // time stamp of last height measurement fusion step
uint32_t HGTmsecTgt = 200; // target interval between height measurement fusion steps
// Estimated time delays (msec) // Estimated time delays (msec)
uint32_t msecVelDelay = 300; uint32_t msecVelDelay = 300;
@ -213,9 +221,4 @@ bool newDataMag;
// AHRS input data variables // AHRS input data variables
float ahrsEul[3]; float ahrsEul[3];
// ADS input data variables
float Veas;
float EAS2TAS; // ratio 0f true to equivalent airspeed
bool newAdsData;
}; };