diff options
4 files changed, 143 insertions, 154 deletions
diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index ec9efe8ee..4d5e56a96 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -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; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3bb395a87..6ca10e56a 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -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_offsets({}), - _accel_offsets({}), - _mag_offsets({}), - - _sensor_combined {}, - - _pos_ref {}, - _baro_ref(0.0f), - _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")), - - /* 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), - - _newDataGps(false), - _newHgtData(false), - _newAdsData(false), - _newDataMag(false), - _newRangeData(false), - - _mavlink_fd(-1), - _parameters {}, - _parameter_handles {}, - _ekf(nullptr) + _gyro({}), + _accel({}), + _mag({}), + _airspeed({}), + _baro({}), + _vstatus({}), + _global_pos({}), + _local_pos({}), + _gps({}), + _wind({}), + _distance {}, + _landDetector {}, + _armed {}, + + _gyro_offsets({}), + _accel_offsets({}), + _mag_offsets({}), + + _sensor_combined {}, + + _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")), + + /* 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), + + _newHgtData(false), + _newAdsData(false), + _newDataMag(false), + _newRangeData(false), + + _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 diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 5c01286e3..c313e83af 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -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(¤t_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, ¤t_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) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index c5517e38b..9b23f4df4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -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) |