dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Reworked the estimator initialization and recovery logic. Should be more resilient to mishaps now

This commit is contained in:
Lorenz Meier 2014-06-03 16:37:58 +02:00
parent b40fcb0aac
commit 94bed70e32
3 changed files with 273 additions and 252 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -577,6 +577,11 @@ FixedwingEstimator::task_main()
bool newAdsData = false;
bool newDataMag = false;
float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
_gps.vel_n_m_s = 0.0f;
_gps.vel_e_m_s = 0.0f;
_gps.vel_d_m_s = 0.0f;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
@ -926,8 +931,15 @@ FixedwingEstimator::task_main()
newDataMag = false;
}
/*
* CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
*/
if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) {
continue;
}
/**
/*
* CHECK IF THE INPUT DATA IS SANE
*/
int check = _ekf->CheckAndBound();
@ -959,6 +971,13 @@ FixedwingEstimator::task_main()
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 4:
{
const char* str = "excessive gyro offsets";
warnx("%s", str);
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
break;
}
default:
{
@ -974,7 +993,7 @@ FixedwingEstimator::task_main()
}
// If non-zero, we got a filter reset
if (check) {
if (check > 0 && check != 3) {
struct ekf_status_report ekf_report;
@ -1013,10 +1032,12 @@ FixedwingEstimator::task_main()
_baro_init = false;
_gps_initialized = false;
_initialized = false;
last_sensor_timestamp = hrt_absolute_time();
last_run = last_sensor_timestamp;
_ekf->ZeroVariables();
_ekf->statesInitialised = false;
_ekf->dtIMU = 0.01f;
// Let the system re-initialize itself
@ -1027,23 +1048,26 @@ FixedwingEstimator::task_main()
/**
* PART TWO: EXECUTE THE FILTER
*
* We run the filter only once all data has been fetched
**/
if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) {
if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
float initVelNED[3];
/* Initialize the filter first */
if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) {
initVelNED[0] = _gps.vel_n_m_s;
initVelNED[1] = _gps.vel_e_m_s;
initVelNED[2] = _gps.vel_d_m_s;
// GPS is in scaled integers, convert
double lat = _gps.lat / 1.0e7;
double lon = _gps.lon / 1.0e7;
float gps_alt = _gps.alt / 1e3f;
initVelNED[0] = _gps.vel_n_m_s;
initVelNED[1] = _gps.vel_e_m_s;
initVelNED[2] = _gps.vel_d_m_s;
// Set up height correctly
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_gps_offset = _baro_ref - _baro.altitude;
@ -1070,10 +1094,13 @@ FixedwingEstimator::task_main()
map_projection_init(&_pos_ref, lat, lon);
mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
#if 0
warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset);
warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination));
#endif
_gps_initialized = true;
@ -1082,22 +1109,18 @@ FixedwingEstimator::task_main()
initVelNED[0] = 0.0f;
initVelNED[1] = 0.0f;
initVelNED[2] = 0.0f;
_ekf->posNED[0] = 0.0f;
_ekf->posNED[1] = 0.0f;
_ekf->posNED[2] = 0.0f;
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
_ekf->posNE[0] = posNED[0];
_ekf->posNE[1] = posNED[1];
_local_pos.ref_alt = _baro_ref;
_baro_gps_offset = 0.0f;
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
}
}
} else if (_ekf->statesInitialised) {
// We're apparently initialized in this case now
// If valid IMU data and states initialised, predict states and covariances
if (_ekf->statesInitialised) {
// Run the strapdown INS equations every IMU update
_ekf->UpdateStrapdownEquationsNED();
#if 0
@ -1134,7 +1157,6 @@ FixedwingEstimator::task_main()
}
_initialized = true;
}
// Fuse GPS Measurements
if (newDataGps && _gps_initialized) {
@ -1142,10 +1164,10 @@ FixedwingEstimator::task_main()
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
_ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
_ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
_ekf->posNE[0] = posNED[0];
_ekf->posNE[1] = posNED[1];
// set fusion flags
_ekf->fuseVelData = true;
_ekf->fusePosData = true;
@ -1160,12 +1182,9 @@ FixedwingEstimator::task_main()
_ekf->velNED[0] = 0.0f;
_ekf->velNED[1] = 0.0f;
_ekf->velNED[2] = 0.0f;
_ekf->posNED[0] = 0.0f;
_ekf->posNED[1] = 0.0f;
_ekf->posNED[2] = 0.0f;
_ekf->posNE[0] = _ekf->posNED[0];
_ekf->posNE[1] = _ekf->posNED[1];
_ekf->posNE[0] = 0.0f;
_ekf->posNE[1] = 0.0f;
// set fusion flags
_ekf->fuseVelData = true;
_ekf->fusePosData = true;
@ -1214,20 +1233,8 @@ FixedwingEstimator::task_main()
_ekf->fuseVtasData = false;
}
// Publish results
if (_initialized && (check == OK)) {
// State vector:
// 0-3: quaternions (q0, q1, q2, q3)
// 4-6: Velocity - m/sec (North, East, Down)
// 7-9: Position - m (North, East, Down)
// 10-12: Delta Angle bias - rad (X,Y,Z)
// 13-14: Wind Vector - m/sec (North,East)
// 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down)
// 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z)
// Output results
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
math::Matrix<3, 3> R = q.to_dcm();
math::Vector<3> euler = R.to_euler();
@ -1265,7 +1272,6 @@ FixedwingEstimator::task_main()
/* advertise and publish */
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
}
}
if (_gps_initialized) {
_local_pos.timestamp = last_sensor_timestamp;
@ -1324,6 +1330,7 @@ FixedwingEstimator::task_main()
_global_pos.vel_d = _local_pos.vz;
}
_global_pos.yaw = _local_pos.yaw;
_global_pos.eph = _gps.eph_m;
@ -1357,7 +1364,13 @@ FixedwingEstimator::task_main()
/* advertise and publish */
_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
}
}
}
}
}
}
@ -1407,9 +1420,10 @@ FixedwingEstimator::print_status()
// 4-6: Velocity - m/sec (North, East, Down)
// 7-9: Position - m (North, East, Down)
// 10-12: Delta Angle bias - rad (X,Y,Z)
// 13-14: Wind Vector - m/sec (North,East)
// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
// 13: Accelerometer offset
// 14-15: Wind Vector - m/sec (North,East)
// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt);

16
src/modules/ekf_att_pos_estimator/estimator.cpp
View File

@ -145,7 +145,7 @@ AttPosEKF::AttPosEKF()
* instead to allow clean in-air re-initialization.
*/
{
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
ZeroVariables();
InitialiseParameters();
}
@ -2382,7 +2382,7 @@ int AttPosEKF::CheckAndBound()
// Reset the filter if the IMU data is too old
if (dtIMU > 0.3f) {
FillErrorReport(&last_ekf_error);
ResetVelocity();
ResetPosition();
ResetHeight();
@ -2397,6 +2397,7 @@ int AttPosEKF::CheckAndBound()
// Check if we switched between states
if (currStaticMode != staticMode) {
FillErrorReport(&last_ekf_error);
ResetVelocity();
ResetPosition();
ResetHeight();
@ -2405,6 +2406,15 @@ int AttPosEKF::CheckAndBound()
return 3;
}
// Reset the filter if gyro offsets are excessive
if (fabs(states[10]) > 1.0f || fabsf(states[11]) > 1.0f || fabsf(states[12]) > 1.0f) {
InitializeDynamic(velNED, magDeclination);
// that's all we can do here, return
return 4;
}
return 0;
}
@ -2531,8 +2541,6 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do
// the baro offset must be this difference now
baroHgtOffset = baroHgt - referenceHgt;
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
InitializeDynamic(initvelNED, declination);
}

1
src/modules/ekf_att_pos_estimator/estimator.h
View File

@ -200,7 +200,6 @@ public:
float hgtMea; // measured height (m)
float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
float rngMea; // Ground distance
float posNED[3]; // North, East Down position (m)
float innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output