fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1913 from Zefz/ekf-fixes

Browse Source 
Fix EKF Attitude Position Estimator bugs
This commit is contained in:
Lorenz Meier 2015-03-14 10:53:19 +01:00
commit d28e4ed7a7
4 changed files with 136 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -174,7 +174,6 @@ private:
struct map_projection_reference_s _pos_ref;
float _baro_ref; /**< barometer reference altitude */
float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */
float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */
hrt_abstime _last_debug_print = 0;
@ -194,6 +193,7 @@ private:
bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use
uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received
bool _baro_init;
float _baroAltRef;
bool _gps_initialized;
hrt_abstime _filter_start_time;
hrt_abstime _last_sensor_timestamp;
@ -208,7 +208,6 @@ private:
bool _ekf_logging; ///< log EKF state
unsigned _debug; ///< debug level - default 0
bool _newDataGps;
bool _newHgtData;
bool _newAdsData;
bool _newDataMag;

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

@ -132,72 +132,71 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_wind_pub(-1),
_att({}),
_gyro({}),
_accel({}),
_mag({}),
_airspeed({}),
_baro({}),
_vstatus({}),
_global_pos({}),
_local_pos({}),
_gps({}),
_wind({}),
_distance {},
_landDetector {},
_armed {},
_gyro({}),
_accel({}),
_mag({}),
_airspeed({}),
_baro({}),
_vstatus({}),
_global_pos({}),
_local_pos({}),
_gps({}),
_wind({}),
_distance {},
_landDetector {},
_armed {},
_gyro_offsets({}),
_accel_offsets({}),
_mag_offsets({}),
_gyro_offsets({}),
_accel_offsets({}),
_mag_offsets({}),
_sensor_combined {},
_sensor_combined {},
_pos_ref {},
_baro_ref(0.0f),
_baro_ref_offset(0.0f),
_baro_gps_offset(0.0f),
_pos_ref{},
_baro_ref_offset(0.0f),
_baro_gps_offset(0.0f),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
_perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
_perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
/* states */
_gps_alt_filt(0.0f),
_baro_alt_filt(0.0f),
_covariancePredictionDt(0.0f),
_gpsIsGood(false),
_previousGPSTimestamp(0),
_baro_init(false),
_gps_initialized(false),
_filter_start_time(0),
_last_sensor_timestamp(0),
_last_run(0),
_distance_last_valid(0),
_gyro_valid(false),
_accel_valid(false),
_mag_valid(false),
_gyro_main(0),
_accel_main(0),
_mag_main(0),
_ekf_logging(true),
_debug(0),
/* states */
_gps_alt_filt(0.0f),
_baro_alt_filt(0.0f),
_covariancePredictionDt(0.0f),
_gpsIsGood(false),
_previousGPSTimestamp(0),
_baro_init(false),
_baroAltRef(0.0f),
_gps_initialized(false),
_filter_start_time(0),
_last_sensor_timestamp(0),
_last_run(0),
_distance_last_valid(0),
_gyro_valid(false),
_accel_valid(false),
_mag_valid(false),
_gyro_main(0),
_accel_main(0),
_mag_main(0),
_ekf_logging(true),
_debug(0),
_newDataGps(false),
_newHgtData(false),
_newAdsData(false),
_newDataMag(false),
_newRangeData(false),
_newHgtData(false),
_newAdsData(false),
_newDataMag(false),
_newRangeData(false),
_mavlink_fd(-1),
_parameters {},
_parameter_handles {},
_ekf(nullptr)
_mavlink_fd(-1),
_parameters {},
_parameter_handles {},
_ekf(nullptr)
{
_last_run = hrt_absolute_time();
@ -636,24 +635,26 @@ void AttitudePositionEstimatorEKF::task_main()
// }
/* Initialize the filter first */
if (!_gps_initialized && _gpsIsGood) {
initializeGPS();
} else if (!_ekf->statesInitialised) {
if (!_ekf->statesInitialised) {
// North, East Down position (m)
float initVelNED[3] = {0.0f, 0.0f, 0.0f};
_ekf->posNE[0] = 0.0f;
_ekf->posNE[1] = 0.0f;
_local_pos.ref_alt = _baro_ref;
_local_pos.ref_alt = 0.0f;
_baro_ref_offset = 0.0f;
_baro_gps_offset = 0.0f;
_baro_alt_filt = _baro.altitude;
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
} else if (_ekf->statesInitialised) {
} else {
if (!_gps_initialized && _gpsIsGood) {
initializeGPS();
continue;
}
// Check if on ground - status is used by covariance prediction
_ekf->setOnGround(_landDetector.landed);
@ -668,7 +669,7 @@ void AttitudePositionEstimatorEKF::task_main()
}
//Run EKF data fusion steps
updateSensorFusion(_newDataGps, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
//Publish attitude estimations
publishAttitude();
@ -717,7 +718,7 @@ void AttitudePositionEstimatorEKF::initializeGPS()
_baro_alt_filt = _baro.altitude;
_ekf->baroHgt = _baro.altitude;
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
_ekf->hgtMea = _ekf->baroHgt;
// Set up position variables correctly
_ekf->GPSstatus = _gps.fix_type;
@ -810,7 +811,7 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
_local_pos.y = _ekf->states[8];
// XXX need to announce change of Z reference somehow elegantly
_local_pos.z = _ekf->states[9] - _baro_ref_offset;
_local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef;
_local_pos.vx = _ekf->states[4];
_local_pos.vy = _ekf->states[5];
@ -858,7 +859,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
}
/* local pos alt is negative, change sign and add alt offsets */
_global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset;
_global_pos.alt = (-_local_pos.z) - _baro_gps_offset;
if (_local_pos.v_z_valid) {
_global_pos.vel_d = _local_pos.vz;
@ -908,8 +909,7 @@ void AttitudePositionEstimatorEKF::publishWindEstimate()
}
void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag,
const bool fuseRangeSensor,
const bool fuseBaro, const bool fuseAirSpeed)
const bool fuseRangeSensor, const bool fuseBaro, const bool fuseAirSpeed)
{
// Run the strapdown INS equations every IMU update
_ekf->UpdateStrapdownEquationsNED();
@ -978,7 +978,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
if (fuseBaro) {
// Could use a blend of GPS and baro alt data if desired
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
_ekf->hgtMea = _ekf->baroHgt;
_ekf->fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
@ -1071,7 +1071,7 @@ void AttitudePositionEstimatorEKF::print_status()
printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f));
printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset,
printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset,
(double)_baro_gps_offset);
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y,
(double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
@ -1268,10 +1268,10 @@ void AttitudePositionEstimatorEKF::pollData()
}
orb_check(_gps_sub, &_newDataGps);
if (_newDataGps) {
bool gps_update;
orb_check(_gps_sub, &gps_update);
if (gps_update) {
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
perf_count(_perf_gps);
@ -1324,10 +1324,7 @@ void AttitudePositionEstimatorEKF::pollData()
if (_gps_initialized) {
//Convert from global frame to local frame
float posNED[3] = {0.0f, 0.0f, 0.0f};
_ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
_ekf->posNE[0] = posNED[0];
_ekf->posNE[1] = posNED[1];
map_projection_project(&_pos_ref, (_gps.lat / 1.0e7), (_gps.lon / 1.0e7), &_ekf->posNE[0], &_ekf->posNE[1]);
if (dtLastGoodGPS > POS_RESET_THRESHOLD) {
_ekf->ResetPosition();
@ -1353,9 +1350,6 @@ void AttitudePositionEstimatorEKF::pollData()
_previousGPSTimestamp = _gps.timestamp_position;
} else {
//Too poor GPS fix to use
_newDataGps = false;
}
}
@ -1406,21 +1400,17 @@ void AttitudePositionEstimatorEKF::pollData()
}
baro_last = _baro.timestamp;
if(!_baro_init) {
_baro_init = true;
_baroAltRef = _baro.altitude;
}
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
_ekf->baroHgt = _baro.altitude;
_baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
if (!_baro_init) {
_baro_ref = _baro.altitude;
_baro_init = true;
warnx("ALT REF INIT");
}
perf_count(_perf_baro);
_newHgtData = true;
}
//Update Magnetometer

91
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
View File

@ -2329,15 +2329,20 @@ void AttPosEKF::zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATE
// Store states in a history array along with time stamp
void AttPosEKF::StoreStates(uint64_t timestamp_ms)
{
for (size_t i=0; i<EKF_STATE_ESTIMATES; i++)
for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
storedStates[i][storeIndex] = states[i];
}
storedOmega[0][storeIndex] = angRate.x;
storedOmega[1][storeIndex] = angRate.y;
storedOmega[2][storeIndex] = angRate.z;
statetimeStamp[storeIndex] = timestamp_ms;
// increment to next storage index
storeIndex++;
if (storeIndex == EKF_DATA_BUFFER_SIZE)
if (storeIndex >= EKF_DATA_BUFFER_SIZE) {
storeIndex = 0;
}
}
void AttPosEKF::ResetStoredStates()
@ -2350,10 +2355,8 @@ void AttPosEKF::ResetStoredStates()
// reset store index to first
storeIndex = 0;
statetimeStamp[storeIndex] = millis();
// increment to next storage index
storeIndex++;
//Reset stored state to current state
StoreStates(millis());
}
// Output the state vector stored at the time that best matches that specified by msec
@ -2513,27 +2516,6 @@ void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4])
y[2] = atan2f((2.0f*(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 AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
{
velNED[0] = gpsGndSpd*cosf(gpsCourse);
velNED[1] = gpsGndSpd*sinf(gpsCourse);
velNED[2] = gpsVelD;
}
void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference)
{
posNED[0] = earthRadius * (lat - latReference);
posNED[1] = earthRadius * cos(latReference) * (lon - lonReference);
posNED[2] = -(hgt - hgtReference);
}
void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
{
lat = latRef + (double)posNED[0] * earthRadiusInv;
lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef);
hgt = hgtRef - posNED[2];
}
void AttPosEKF::setOnGround(const bool isLanded)
{
_onGround = isLanded;
@ -2592,25 +2574,40 @@ void AttPosEKF::CovarianceInit()
P[1][1] = 0.25f * sq(1.0f*deg2rad);
P[2][2] = 0.25f * sq(1.0f*deg2rad);
P[3][3] = 0.25f * sq(10.0f*deg2rad);
//velocities
P[4][4] = sq(0.7f);
P[5][5] = P[4][4];
P[6][6] = sq(0.7f);
//positions
P[7][7] = sq(15.0f);
P[8][8] = P[7][7];
P[9][9] = sq(5.0f);
//delta angle biases
P[10][10] = sq(0.1f*deg2rad*dtIMU);
P[11][11] = P[10][10];
P[12][12] = P[10][10];
//Z delta velocity bias
P[13][13] = sq(0.2f*dtIMU);
P[14][14] = sq(0.0f);
//Wind velocities
P[14][14] = 0.0f;
P[15][15] = P[14][14];
//Earth magnetic field
P[16][16] = sq(0.02f);
P[17][17] = P[16][16];
P[18][18] = P[16][16];
//Body magnetic field
P[19][19] = sq(0.02f);
P[20][20] = P[19][19];
P[21][21] = P[19][19];
//Optical flow
fScaleFactorVar = 0.001f; // focal length scale factor variance
Popt[0][0] = 0.001f;
}
@ -2628,9 +2625,11 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val)
ret = val;
}
#if 0
if (!isfinite(val)) {
//ekf_debug("constrain: non-finite!");
ekf_debug("constrain: non-finite!");
}
#endif
return ret;
}
@ -2863,8 +2862,12 @@ void AttPosEKF::ResetPosition(void)
for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
storedStates[7][i] = states[7];
storedStates[8][i] = states[8];
}
}
}
//reset position covariance
P[7][7] = sq(15.0f);
P[8][8] = P[7][7];
}
void AttPosEKF::ResetHeight(void)
@ -2876,6 +2879,10 @@ void AttPosEKF::ResetHeight(void)
for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
storedStates[9][i] = states[9];
}
//reset altitude covariance
P[9][9] = sq(5.0f);
P[6][6] = sq(0.7f);
}
void AttPosEKF::ResetVelocity(void)
@ -2884,7 +2891,8 @@ void AttPosEKF::ResetVelocity(void)
states[4] = 0.0f;
states[5] = 0.0f;
states[6] = 0.0f;
} else if (GPSstatus >= GPS_FIX_3D) {
}
else if (GPSstatus >= GPS_FIX_3D) {
//Do not use Z velocity, we trust the Barometer history more
states[4] = velNED[0]; // north velocity from last reading
@ -2894,8 +2902,12 @@ void AttPosEKF::ResetVelocity(void)
for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
storedStates[4][i] = states[4];
storedStates[5][i] = states[5];
}
}
}
//reset velocities covariance
P[4][4] = sq(0.7f);
P[5][5] = P[4][4];
}
bool AttPosEKF::StatesNaN() {
@ -3012,10 +3024,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
// Fill error report
GetFilterState(&last_ekf_error);
ResetStoredStates();
ResetVelocity();
ResetPosition();
ResetHeight();
ResetStoredStates();
// Timeout cleared with this reset
current_ekf_state.imuTimeout = false;
@ -3029,10 +3041,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
// Fill error report, but not setting error flag
GetFilterState(&last_ekf_error);
ResetStoredStates();
ResetVelocity();
ResetPosition();
ResetHeight();
ResetStoredStates();
ret = 0;
}
@ -3202,10 +3214,10 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
states[20] = magBias.y; // Magnetic Field Bias Y
states[21] = magBias.z; // Magnetic Field Bias Z
ResetStoredStates();
ResetVelocity();
ResetPosition();
ResetHeight();
ResetStoredStates();
// initialise focal length scale factor estimator states
flowStates[0] = 1.0f;
@ -3217,7 +3229,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
//Define Earth rotation vector in the NED navigation frame
calcEarthRateNED(earthRateNED, latRef);
}
void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination)
@ -3293,7 +3304,6 @@ void AttPosEKF::ZeroVariables()
magstate.DCM.identity();
memset(&current_ekf_state, 0, sizeof(current_ekf_state));
}
void AttPosEKF::GetFilterState(struct ekf_status_report *err)
@ -3310,13 +3320,6 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err)
current_ekf_state.useAirspeed = useAirspeed;
memcpy(err, &current_ekf_state, sizeof(*err));
// err->velHealth = current_ekf_state.velHealth;
// err->posHealth = current_ekf_state.posHealth;
// err->hgtHealth = current_ekf_state.hgtHealth;
// err->velTimeout = current_ekf_state.velTimeout;
// err->posTimeout = current_ekf_state.posTimeout;
// err->hgtTimeout = current_ekf_state.hgtTimeout;
}
void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)

17
src/modules/ekf_att_pos_estimator/estimator_22states.h
View File

@ -288,7 +288,6 @@ public:
* Recall the state vector.
*
* Recalls the vector stored at closest time to the one specified by msec
*FuseOptFlow
* @return zero on success, integer indicating the number of invalid states on failure.
* Does only copy valid states, if the statesForFusion vector was initialized
* correctly by the caller, the result can be safely used, but is a mixture
@ -307,12 +306,6 @@ public:
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
//static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
static inline float sq(float valIn) {return valIn * valIn;}
@ -362,8 +355,6 @@ public:
*/
void ResetVelocity();
void ZeroVariables();
void GetFilterState(struct ekf_status_report *state);
void GetLastErrorState(struct ekf_status_report *last_error);
@ -381,6 +372,12 @@ public:
* true if the vehicle moves like a Fixed Wing, false otherwise
**/
void setIsFixedWing(const bool fixedWing);
/**
* @brief
* Reset internal filter states and clear all variables to zero value
*/
void ZeroVariables();
protected:
@ -409,7 +406,7 @@ protected:
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
void ResetStoredStates();
private:
bool _isFixedWing; ///< True if the vehicle is a fixed-wing frame type
bool _onGround; ///< boolean true when the flight vehicle is on the ground (not flying)