23 state estimator: Prepare GPS accel init (not yet active), clean up init calls

This commit is contained in:
Lorenz Meier 2014-06-22 16:51:48 +02:00
parent 1018dec246
commit ac319a7240
2 changed files with 23 additions and 13 deletions

View File

@ -38,8 +38,12 @@ AttPosEKF::AttPosEKF()
velNED[0] = 0.0f; velNED[0] = 0.0f;
velNED[1] = 0.0f; velNED[1] = 0.0f;
velNED[2] = 0.0f; velNED[2] = 0.0f;
accelGPSNED[0] = 0.0f;
accelGPSNED[1] = 0.0f;
accelGPSNED[2] = 0.0f;
delAngTotal.zero(); delAngTotal.zero();
ekfDiverged = false; ekfDiverged = false;
lastReset = 0;
memset(&last_ekf_error, 0, sizeof(last_ekf_error)); memset(&last_ekf_error, 0, sizeof(last_ekf_error));
ZeroVariables(); ZeroVariables();
@ -2321,12 +2325,21 @@ int AttPosEKF::CheckAndBound()
// Store the old filter state // Store the old filter state
bool currStaticMode = staticMode; bool currStaticMode = staticMode;
// Limit reset rate to 5 Hz to allow the filter
// to settle
if (millis() - lastReset < 200) {
return 0;
}
if (ekfDiverged) { if (ekfDiverged) {
ekfDiverged = false; ekfDiverged = false;
} }
int ret = 0; int ret = 0;
// Check if we're on ground - this also sets static mode.
OnGroundCheck();
// Reset the filter if the states went NaN // Reset the filter if the states went NaN
if (StatesNaN(&last_ekf_error)) { if (StatesNaN(&last_ekf_error)) {
ekf_debug("re-initializing dynamic"); ekf_debug("re-initializing dynamic");
@ -2348,9 +2361,6 @@ int AttPosEKF::CheckAndBound()
ret = 2; ret = 2;
} }
// Check if we're on ground - this also sets static mode.
OnGroundCheck();
// Check if we switched between states // Check if we switched between states
if (currStaticMode != staticMode) { if (currStaticMode != staticMode) {
FillErrorReport(&last_ekf_error); FillErrorReport(&last_ekf_error);
@ -2388,6 +2398,7 @@ int AttPosEKF::CheckAndBound()
if (ret) { if (ret) {
ekfDiverged = true; ekfDiverged = true;
lastReset = millis();
} }
return ret; return ret;
@ -2441,7 +2452,6 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f
void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
{ {
ZeroVariables(); ZeroVariables();
// Fill variables with valid data // Fill variables with valid data
@ -2478,17 +2488,13 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
magstate.R_MAG = sq(magMeasurementSigma); magstate.R_MAG = sq(magMeasurementSigma);
magstate.DCM = DCM; magstate.DCM = DCM;
// Calculate position from gps measurement
float posNEDInit[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
calcposNED(posNEDInit, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
// 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
for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities
// positions: // positions:
states[7] = posNEDInit[0]; states[7] = posNE[0];
states[8] = posNEDInit[1]; states[8] = posNE[1];
states[9] = posNEDInit[2]; states[9] = -hgtMea;
for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel
states[16] = initMagNED.x; // Magnetic Field North states[16] = initMagNED.x; // Magnetic Field North
states[17] = initMagNED.y; // Magnetic Field East states[17] = initMagNED.y; // Magnetic Field East
@ -2520,11 +2526,13 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do
hgtRef = referenceHgt; hgtRef = referenceHgt;
refSet = true; refSet = true;
// we are at reference altitude, so measurement must be zero // we are at reference position, so measurement must be zero
hgtMea = 0.0f;
posNE[0] = 0.0f; posNE[0] = 0.0f;
posNE[1] = 0.0f; posNE[1] = 0.0f;
// we are at an unknown, possibly non-zero altitude - so altitude
// is not reset (hgtMea)
// the baro offset must be this difference now // the baro offset must be this difference now
baroHgtOffset = baroHgt - referenceHgt; baroHgtOffset = baroHgt - referenceHgt;

View File

@ -138,6 +138,7 @@ public:
float varInnovVelPos[6]; // innovation variance output float varInnovVelPos[6]; // innovation variance output
float velNED[3]; // North, East, Down velocity obs (m/s) float velNED[3]; // North, East, Down velocity obs (m/s)
float accelGPSNED[3]; // Acceleration predicted by GPS in earth frame
float posNE[2]; // North, East position obs (m) float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m) float hgtMea; // measured height (m)
float baroHgtOffset; ///< the baro (weather) offset from normalized altitude float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
@ -185,6 +186,7 @@ public:
bool useRangeFinder; ///< true when rangefinder is being used bool useRangeFinder; ///< true when rangefinder is being used
bool ekfDiverged; bool ekfDiverged;
uint64_t lastReset;
struct ekf_status_report current_ekf_state; struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error; struct ekf_status_report last_ekf_error;