mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-02 09:31:36 +08:00
ekf2 initialization fixes
This commit is contained in:
Daniel Agar
@@ -47,7 +47,7 @@ class Ekf : public EstimatorInterface
|
||||
{
|
||||
public:
|
||||
|
||||
Ekf();
|
||||
Ekf() = default;
|
||||
~Ekf() = default;
|
||||
|
||||
// initialise variables to sane values (also interface class)
|
||||
@@ -80,10 +80,10 @@ public:
|
||||
void get_airspeed_innov_var(float *airspeed_innov_var);
|
||||
|
||||
// gets the innovations of synthetic sideslip measurement
|
||||
void get_beta_innov(float *beta_innov);
|
||||
void get_beta_innov(float *beta_innov);
|
||||
|
||||
// gets the innovation variance of the synthetic sideslip measurement
|
||||
void get_beta_innov_var(float *beta_innov_var);
|
||||
// gets the innovation variance of the synthetic sideslip measurement
|
||||
void get_beta_innov_var(float *beta_innov_var);
|
||||
|
||||
// gets the innovation variance of the heading measurement
|
||||
void get_heading_innov_var(float *heading_innov_var);
|
||||
@@ -165,7 +165,7 @@ public:
|
||||
void get_gyro_bias(float bias[3]);
|
||||
|
||||
// get GPS check status
|
||||
void get_gps_check_status(uint16_t *_gps_check_fail_status);
|
||||
void get_gps_check_status(uint16_t *val);
|
||||
|
||||
// return the amount the local vertical position changed in the last reset and the number of reset events
|
||||
void get_posD_reset(float *delta, uint8_t *counter) {*delta = _state_reset_status.posD_change; *counter = _state_reset_status.posD_counter;}
|
||||
@@ -174,13 +174,15 @@ public:
|
||||
void get_velD_reset(float *delta, uint8_t *counter) {*delta = _state_reset_status.velD_change; *counter = _state_reset_status.velD_counter;}
|
||||
|
||||
// return the amount the local horizontal position changed in the last reset and the number of reset events
|
||||
void get_posNE_reset(float delta[2], uint8_t *counter){
|
||||
void get_posNE_reset(float delta[2], uint8_t *counter)
|
||||
{
|
||||
memcpy(delta, &_state_reset_status.posNE_change._data[0], sizeof(_state_reset_status.posNE_change._data));
|
||||
*counter = _state_reset_status.posNE_counter;
|
||||
}
|
||||
|
||||
// return the amount the local horizontal velocity changed in the last reset and the number of reset events
|
||||
void get_velNE_reset(float delta[2], uint8_t *counter) {
|
||||
void get_velNE_reset(float delta[2], uint8_t *counter)
|
||||
{
|
||||
memcpy(delta, &_state_reset_status.velNE_change._data[0], sizeof(_state_reset_status.velNE_change._data));
|
||||
*counter = _state_reset_status.velNE_counter;
|
||||
}
|
||||
@@ -204,9 +206,9 @@ public:
|
||||
|
||||
private:
|
||||
|
||||
static const uint8_t _k_num_states = 24;
|
||||
static const float _k_earth_rate;
|
||||
static const float _gravity_mss;
|
||||
static constexpr uint8_t _k_num_states{24};
|
||||
static constexpr float _k_earth_rate{0.000072921f};
|
||||
static constexpr float _gravity_mss{9.80665f};
|
||||
|
||||
// reset event monitoring
|
||||
// structure containing velocity, position, height and yaw reset information
|
||||
@@ -221,145 +223,145 @@ private:
|
||||
Vector2f posNE_change; // North, East position change due to last reset (m)
|
||||
float posD_change; // Down position change due to last reset (m)
|
||||
Quaternion quat_change; // quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion
|
||||
} _state_reset_status;
|
||||
} _state_reset_status{};
|
||||
|
||||
float _dt_ekf_avg; // average update rate of the ekf
|
||||
float _dt_update; // delta time since last ekf update. This time can be used for filters
|
||||
// which run at the same rate as the Ekf::update() function
|
||||
float _dt_ekf_avg{0.001f * FILTER_UPDATE_PERIOD_MS}; // average update rate of the ekf
|
||||
float _dt_update{0.01f}; // delta time since last ekf update. This time can be used for filters
|
||||
// which run at the same rate as the Ekf::update() function
|
||||
|
||||
stateSample _state; // state struct of the ekf running at the delayed time horizon
|
||||
stateSample _state{}; // state struct of the ekf running at the delayed time horizon
|
||||
|
||||
bool _filter_initialised; // true when the EKF sttes and covariances been initialised
|
||||
bool _earth_rate_initialised; // true when we know the earth rotatin rate (requires GPS)
|
||||
bool _filter_initialised{false}; // true when the EKF sttes and covariances been initialised
|
||||
bool _earth_rate_initialised{false}; // true when we know the earth rotatin rate (requires GPS)
|
||||
|
||||
bool _fuse_height; // baro height data should be fused
|
||||
bool _fuse_pos; // gps position data should be fused
|
||||
bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused
|
||||
bool _fuse_vert_vel; // gps vertical velocity measurement should be fused
|
||||
bool _fuse_height{false}; // baro height data should be fused
|
||||
bool _fuse_pos{false}; // gps position data should be fused
|
||||
bool _fuse_hor_vel{false}; // gps horizontal velocity measurement should be fused
|
||||
bool _fuse_vert_vel{false}; // gps vertical velocity measurement should be fused
|
||||
|
||||
// booleans true when fresh sensor data is available at the fusion time horizon
|
||||
bool _gps_data_ready;
|
||||
bool _mag_data_ready;
|
||||
bool _baro_data_ready;
|
||||
bool _range_data_ready;
|
||||
bool _flow_data_ready;
|
||||
bool _ev_data_ready;
|
||||
bool _tas_data_ready;
|
||||
bool _gps_data_ready{false};
|
||||
bool _mag_data_ready{false};
|
||||
bool _baro_data_ready{false};
|
||||
bool _range_data_ready{false};
|
||||
bool _flow_data_ready{false};
|
||||
bool _ev_data_ready{false};
|
||||
bool _tas_data_ready{false};
|
||||
|
||||
uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode
|
||||
uint64_t _time_last_fake_gps{0}; // last time in us at which we have faked gps measurement for static mode
|
||||
|
||||
uint64_t _time_last_pos_fuse; // time the last fusion of horizontal position measurements was performed (usec)
|
||||
uint64_t _time_last_vel_fuse; // time the last fusion of velocity measurements was performed (usec)
|
||||
uint64_t _time_last_hgt_fuse; // time the last fusion of height measurements was performed (usec)
|
||||
uint64_t _time_last_of_fuse; // time the last fusion of optical flow measurements were performed (usec)
|
||||
uint64_t _time_last_arsp_fuse; // time the last fusion of airspeed measurements were performed (usec)
|
||||
uint64_t _time_last_beta_fuse; // time the last fusion of synthetic sideslip measurements were performed (usec)
|
||||
uint64_t _time_last_pos_fuse{0}; // time the last fusion of horizontal position measurements was performed (usec)
|
||||
uint64_t _time_last_vel_fuse{0}; // time the last fusion of velocity measurements was performed (usec)
|
||||
uint64_t _time_last_hgt_fuse{0}; // time the last fusion of height measurements was performed (usec)
|
||||
uint64_t _time_last_of_fuse{0}; // time the last fusion of optical flow measurements were performed (usec)
|
||||
uint64_t _time_last_arsp_fuse{0}; // time the last fusion of airspeed measurements were performed (usec)
|
||||
uint64_t _time_last_beta_fuse{0}; // time the last fusion of synthetic sideslip measurements were performed (usec)
|
||||
Vector2f _last_known_posNE; // last known local NE position vector (m)
|
||||
float _last_disarmed_posD; // vertical position recorded at arming (m)
|
||||
float _imu_collection_time_adj; // the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
|
||||
float _last_disarmed_posD{0.0f}; // vertical position recorded at arming (m)
|
||||
float _imu_collection_time_adj{0.0f}; // the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
|
||||
|
||||
uint64_t _time_acc_bias_check; // last time the accel bias check passed (usec)
|
||||
uint64_t _time_acc_bias_check{0}; // last time the accel bias check passed (usec)
|
||||
|
||||
Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s
|
||||
|
||||
matrix::Dcm<float> _R_to_earth; // transformation matrix from body frame to earth frame from last EKF predition
|
||||
|
||||
float P[_k_num_states][_k_num_states]{}; // state covariance matrix
|
||||
float P[_k_num_states][_k_num_states] {}; // state covariance matrix
|
||||
|
||||
float _vel_pos_innov[6]{}; // innovations: 0-2 vel, 3-5 pos
|
||||
float _vel_pos_innov_var[6]{}; // innovation variances: 0-2 vel, 3-5 pos
|
||||
float _vel_pos_innov[6] {}; // innovations: 0-2 vel, 3-5 pos
|
||||
float _vel_pos_innov_var[6] {}; // innovation variances: 0-2 vel, 3-5 pos
|
||||
|
||||
float _mag_innov[3]{}; // earth magnetic field innovations
|
||||
float _mag_innov_var[3]{}; // earth magnetic field innovation variance
|
||||
float _mag_innov[3] {}; // earth magnetic field innovations
|
||||
float _mag_innov_var[3] {}; // earth magnetic field innovation variance
|
||||
|
||||
float _airspeed_innov; // airspeed measurement innovation
|
||||
float _airspeed_innov_var; // airspeed measurement innovation variance
|
||||
float _airspeed_innov{0.0f}; // airspeed measurement innovation
|
||||
float _airspeed_innov_var{0.0f}; // airspeed measurement innovation variance
|
||||
|
||||
float _beta_innov; // synthetic sideslip measurement innovation
|
||||
float _beta_innov_var; // synthetic sideslip measurement innovation variance
|
||||
float _beta_innov{0.0f}; // synthetic sideslip measurement innovation
|
||||
float _beta_innov_var{0.0f}; // synthetic sideslip measurement innovation variance
|
||||
|
||||
float _drag_innov[2]; // multirotor drag measurement innovation
|
||||
float _drag_innov_var[2]; // multirotor drag measurement innovation variance
|
||||
float _drag_innov[2] {}; // multirotor drag measurement innovation
|
||||
float _drag_innov_var[2] {}; // multirotor drag measurement innovation variance
|
||||
|
||||
float _heading_innov; // heading measurement innovation
|
||||
float _heading_innov_var; // heading measurement innovation variance
|
||||
float _heading_innov{0.0f}; // heading measurement innovation
|
||||
float _heading_innov_var{0.0f}; // heading measurement innovation variance
|
||||
|
||||
// optical flow processing
|
||||
float _flow_innov[2]{}; // flow measurement innovation
|
||||
float _flow_innov_var[2]{}; // flow innovation variance
|
||||
float _flow_innov[2] {}; // flow measurement innovation
|
||||
float _flow_innov_var[2] {}; // flow innovation variance
|
||||
Vector3f _flow_gyro_bias; // bias errors in optical flow sensor rate gyro outputs
|
||||
Vector3f _imu_del_ang_of; // bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates
|
||||
float _delta_time_of; // time in sec that _imu_del_ang_of was accumulated over
|
||||
float _delta_time_of{0.0f}; // time in sec that _imu_del_ang_of was accumulated over
|
||||
|
||||
float _mag_declination; // magnetic declination used by reset and fusion functions (rad)
|
||||
float _mag_declination{0.0f}; // magnetic declination used by reset and fusion functions (rad)
|
||||
|
||||
// output predictor states
|
||||
Vector3f _delta_angle_corr; // delta angle correction vector
|
||||
imuSample _imu_down_sampled; // down sampled imu data (sensor rate -> filter update rate)
|
||||
imuSample _imu_down_sampled{}; // down sampled imu data (sensor rate -> filter update rate)
|
||||
Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps)
|
||||
Vector3f _vel_err_integ; // integral of velocity tracking error
|
||||
Vector3f _pos_err_integ; // integral of position tracking error
|
||||
float _output_tracking_error[3]{};// contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
|
||||
float _output_tracking_error[3] {}; // contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
|
||||
|
||||
// variables used for the GPS quality checks
|
||||
float _gpsDriftVelN; // GPS north position derivative (m/s)
|
||||
float _gpsDriftVelE; // GPS east position derivative (m/s)
|
||||
float _gps_drift_velD; // GPS down position derivative (m/s)
|
||||
float _gps_velD_diff_filt; // GPS filtered Down velocity (m/s)
|
||||
float _gps_velN_filt; // GPS filtered North velocity (m/s)
|
||||
float _gps_velE_filt; // GPS filtered East velocity (m/s)
|
||||
uint64_t _last_gps_fail_us; // last system time in usec that the GPS failed it's checks
|
||||
float _gpsDriftVelN{0.0f}; // GPS north position derivative (m/s)
|
||||
float _gpsDriftVelE{0.0f}; // GPS east position derivative (m/s)
|
||||
float _gps_drift_velD{0.0f}; // GPS down position derivative (m/s)
|
||||
float _gps_velD_diff_filt{0.0f}; // GPS filtered Down velocity (m/s)
|
||||
float _gps_velN_filt{0.0f}; // GPS filtered North velocity (m/s)
|
||||
float _gps_velE_filt{0.0f}; // GPS filtered East velocity (m/s)
|
||||
uint64_t _last_gps_fail_us{0}; // last system time in usec that the GPS failed it's checks
|
||||
|
||||
// Variables used to publish the WGS-84 location of the EKF local NED origin
|
||||
uint64_t _last_gps_origin_time_us; // time the origin was last set (uSec)
|
||||
float _gps_alt_ref; // WGS-84 height (m)
|
||||
uint64_t _last_gps_origin_time_us{0}; // time the origin was last set (uSec)
|
||||
float _gps_alt_ref{0.0f}; // WGS-84 height (m)
|
||||
|
||||
// Variables used to initialise the filter states
|
||||
uint32_t _hgt_counter; // number of height samples read during initialisation
|
||||
float _rng_filt_state; // filtered height measurement
|
||||
uint32_t _mag_counter; // number of magnetometer samples read during initialisation
|
||||
uint32_t _ev_counter; // number of exgernal vision samples read during initialisation
|
||||
uint64_t _time_last_mag; // measurement time of last magnetomter sample
|
||||
uint32_t _hgt_counter{0}; // number of height samples read during initialisation
|
||||
float _rng_filt_state{0.0f}; // filtered height measurement
|
||||
uint32_t _mag_counter{0}; // number of magnetometer samples read during initialisation
|
||||
uint32_t _ev_counter{0}; // number of exgernal vision samples read during initialisation
|
||||
uint64_t _time_last_mag{0}; // measurement time of last magnetomter sample
|
||||
Vector3f _mag_filt_state; // filtered magnetometer measurement
|
||||
Vector3f _delVel_sum; // summed delta velocity
|
||||
float _hgt_sensor_offset; // set as necessary if desired to maintain the same height after a height reset (m)
|
||||
float _baro_hgt_offset; // baro height reading at the local NED origin (m)
|
||||
float _hgt_sensor_offset{0.0f}; // set as necessary if desired to maintain the same height after a height reset (m)
|
||||
float _baro_hgt_offset{0.0f}; // baro height reading at the local NED origin (m)
|
||||
|
||||
// Variables used to control activation of post takeoff functionality
|
||||
float _last_on_ground_posD; // last vertical position when the in_air status was false (m)
|
||||
float _last_on_ground_posD{0.0f}; // last vertical position when the in_air status was false (m)
|
||||
|
||||
gps_check_fail_status_u _gps_check_fail_status{};
|
||||
|
||||
// variables used to inhibit accel bias learning
|
||||
bool _accel_bias_inhibit; // true when the accel bias learning is being inhibited
|
||||
float _accel_mag_filt; // acceleration magnitude after application of a decaying envelope filter (m/sec**2)
|
||||
float _ang_rate_mag_filt; // angular rate magnitude after application of a decaying envelope filter (rad/sec)
|
||||
bool _accel_bias_inhibit{false}; // true when the accel bias learning is being inhibited
|
||||
float _accel_mag_filt{0.0f}; // acceleration magnitude after application of a decaying envelope filter (m/sec**2)
|
||||
float _ang_rate_mag_filt{0.0f}; // angular rate magnitude after application of a decaying envelope filter (rad/sec)
|
||||
Vector3f _prev_dvel_bias_var; // saved delta velocity XYZ bias variances (m/sec)**2
|
||||
|
||||
// Terrain height state estimation
|
||||
float _terrain_vpos; // estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
|
||||
float _terrain_var; // variance of terrain position estimate (m^2)
|
||||
float _hagl_innov; // innovation of the last height above terrain measurement (m)
|
||||
float _hagl_innov_var; // innovation variance for the last height above terrain measurement (m^2)
|
||||
float _terrain_vpos{0.0f}; // estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
|
||||
float _terrain_var{1e4f}; // variance of terrain position estimate (m^2)
|
||||
float _hagl_innov{0.0f}; // innovation of the last height above terrain measurement (m)
|
||||
float _hagl_innov_var{0.0f}; // innovation variance for the last height above terrain measurement (m^2)
|
||||
uint64_t _time_last_hagl_fuse; // last system time in usec that the hagl measurement failed it's checks
|
||||
bool _terrain_initialised; // true when the terrain estimator has been intialised
|
||||
float _sin_tilt_rng; // sine of the range finder tilt rotation about the Y body axis
|
||||
float _cos_tilt_rng; // cosine of the range finder tilt rotation about the Y body axis
|
||||
float _R_rng_to_earth_2_2; // 2,2 element of the rotation matrix from sensor frame to earth frame
|
||||
bool _range_data_continuous; // true when we are receiving range finder data faster than a 2Hz average
|
||||
float _dt_last_range_update_filt_us; // filtered value of the delta time elapsed since the last range measurement came into
|
||||
// the filter (microseconds)
|
||||
bool _terrain_initialised{false}; // true when the terrain estimator has been intialised
|
||||
float _sin_tilt_rng{0.0f}; // sine of the range finder tilt rotation about the Y body axis
|
||||
float _cos_tilt_rng{0.0f}; // cosine of the range finder tilt rotation about the Y body axis
|
||||
float _R_rng_to_earth_2_2{0.0f}; // 2,2 element of the rotation matrix from sensor frame to earth frame
|
||||
bool _range_data_continuous{false}; // true when we are receiving range finder data faster than a 2Hz average
|
||||
float _dt_last_range_update_filt_us{0.0f}; // filtered value of the delta time elapsed since the last range measurement came into
|
||||
// the filter (microseconds)
|
||||
|
||||
// height sensor fault status
|
||||
bool _baro_hgt_faulty; // true if valid baro data is unavailable for use
|
||||
bool _gps_hgt_faulty; // true if valid gps height data is unavailable for use
|
||||
bool _rng_hgt_faulty; // true if valid rnage finder height data is unavailable for use
|
||||
int _primary_hgt_source; // priary source of height data set at initialisation
|
||||
bool _baro_hgt_faulty{false}; // true if valid baro data is unavailable for use
|
||||
bool _gps_hgt_faulty{false}; // true if valid gps height data is unavailable for use
|
||||
bool _rng_hgt_faulty{false}; // true if valid rnage finder height data is unavailable for use
|
||||
int _primary_hgt_source{VDIST_SENSOR_BARO}; // priary source of height data set at initialisation
|
||||
|
||||
// imu fault status
|
||||
uint64_t _time_bad_vert_accel; // last time a bad vertical accel was detected (usec)
|
||||
uint64_t _time_good_vert_accel; // last time a good vertical accel was detected (usec)
|
||||
bool _bad_vert_accel_detected; // true when bad vertical accelerometer data has been detected
|
||||
uint64_t _time_bad_vert_accel{0}; // last time a bad vertical accel was detected (usec)
|
||||
uint64_t _time_good_vert_accel{0}; // last time a good vertical accel was detected (usec)
|
||||
bool _bad_vert_accel_detected{false}; // true when bad vertical accelerometer data has been detected
|
||||
|
||||
// update the real time complementary filter states. This includes the prediction
|
||||
// and the correction step
|
||||
@@ -390,7 +392,7 @@ private:
|
||||
void fuseAirspeed();
|
||||
|
||||
// fuse synthetic zero sideslip measurement
|
||||
void fuseSideslip();
|
||||
void fuseSideslip();
|
||||
|
||||
// fuse body frame drag specific forces for multi-rotor wind estimation
|
||||
void fuseDrag();
|
||||
|
||||
Reference in New Issue
Block a user