mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-26 01:17:51 +08:00
Delete redundant Ekf class member variable constructor initilizations and add missing C++ style initializers to the header file.
This commit is contained in:
mcsauder
Daniel Agar
@@ -68,12 +68,6 @@ void Ekf::reset()
|
||||
|
||||
_delta_angle_corr.setZero();
|
||||
|
||||
_imu_updated = false;
|
||||
_NED_origin_initialised = false;
|
||||
_gps_speed_valid = false;
|
||||
|
||||
_filter_initialised = false;
|
||||
_terrain_initialised = false;
|
||||
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
|
||||
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
|
||||
_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
|
||||
@@ -81,19 +75,13 @@ void Ekf::reset()
|
||||
_control_status.value = 0;
|
||||
_control_status_prev.value = 0;
|
||||
|
||||
_dt_ekf_avg = FILTER_UPDATE_PERIOD_S;
|
||||
|
||||
_ang_rate_delayed_raw.zero();
|
||||
|
||||
_fault_status.value = 0;
|
||||
_innov_check_fail_status.value = 0;
|
||||
|
||||
_accel_magnitude_filt = 0.0f;
|
||||
_ang_rate_magnitude_filt = 0.0f;
|
||||
_prev_dvel_bias_var.zero();
|
||||
|
||||
_gps_alt_ref = 0.0f;
|
||||
|
||||
resetGpsDriftCheckFilters();
|
||||
}
|
||||
|
||||
|
||||
+62
-58
@@ -51,11 +51,13 @@ class Ekf final : public EstimatorInterface
|
||||
{
|
||||
public:
|
||||
static constexpr uint8_t _k_num_states{24}; ///< number of EKF states
|
||||
|
||||
typedef matrix::Vector<float, _k_num_states> Vector24f;
|
||||
typedef matrix::SquareMatrix<float, _k_num_states> SquareMatrix24f;
|
||||
typedef matrix::SquareMatrix<float, 2> Matrix2f;
|
||||
typedef matrix::Vector<float, 4> Vector4f;
|
||||
template<int ... Idxs>
|
||||
|
||||
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
|
||||
|
||||
Ekf() = default;
|
||||
@@ -90,6 +92,7 @@ public:
|
||||
void getFlowInnov(float flow_innov[2]) const { _flow_innov.copyTo(flow_innov); }
|
||||
void getFlowInnovVar(float flow_innov_var[2]) const { _flow_innov_var.copyTo(flow_innov_var); }
|
||||
void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = _optflow_test_ratio; }
|
||||
|
||||
const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
|
||||
const Vector2f &getFlowVelNE() const { return _flow_vel_ne; }
|
||||
const Vector2f &getFlowCompensated() const { return _flow_compensated_XY_rad; }
|
||||
@@ -187,14 +190,12 @@ public:
|
||||
// error magnitudes (rad), (m/sec), (m)
|
||||
const Vector3f &getOutputTrackingError() const { return _output_tracking_error; }
|
||||
|
||||
/*
|
||||
First argument returns GPS drift metrics in the following array locations
|
||||
0 : Horizontal position drift rate (m/s)
|
||||
1 : Vertical position drift rate (m/s)
|
||||
2 : Filtered horizontal velocity (m/s)
|
||||
Second argument returns true when IMU movement is blocking the drift calculation
|
||||
Function returns true if the metrics have been updated and not returned previously by this function
|
||||
*/
|
||||
// First argument returns GPS drift metrics in the following array locations
|
||||
// 0 : Horizontal position drift rate (m/s)
|
||||
// 1 : Vertical position drift rate (m/s)
|
||||
// 2 : Filtered horizontal velocity (m/s)
|
||||
// Second argument returns true when IMU movement is blocking the drift calculation
|
||||
// Function returns true if the metrics have been updated and not returned previously by this function
|
||||
bool get_gps_drift_metrics(float drift[3], bool *blocked);
|
||||
|
||||
// return true if the global position estimate is valid
|
||||
@@ -330,7 +331,7 @@ private:
|
||||
|
||||
float _dt_ekf_avg{FILTER_UPDATE_PERIOD_S}; ///< average update rate of the ekf
|
||||
|
||||
Vector3f _ang_rate_delayed_raw; ///< uncorrected angular rate vector at fusion time horizon (rad/sec)
|
||||
Vector3f _ang_rate_delayed_raw{}; ///< uncorrected angular rate vector at fusion time horizon (rad/sec)
|
||||
|
||||
stateSample _state{}; ///< state struct of the ekf running at the delayed time horizon
|
||||
|
||||
@@ -338,8 +339,8 @@ private:
|
||||
|
||||
// variables used when position data is being fused using a relative position odometry model
|
||||
bool _fuse_hpos_as_odom{false}; ///< true when the NE position data is being fused using an odometry assumption
|
||||
Vector3f _pos_meas_prev; ///< previous value of NED position measurement fused using odometry assumption (m)
|
||||
Vector2f _hpos_pred_prev; ///< previous value of NE position state used by odometry fusion (m)
|
||||
Vector3f _pos_meas_prev{}; ///< previous value of NED position measurement fused using odometry assumption (m)
|
||||
Vector2f _hpos_pred_prev{}; ///< previous value of NE position state used by odometry fusion (m)
|
||||
bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use
|
||||
Dcmf _R_ev_to_ekf; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
|
||||
bool _inhibit_ev_yaw_use{false}; ///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
|
||||
@@ -373,18 +374,18 @@ private:
|
||||
uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
|
||||
|
||||
|
||||
Vector2f _last_known_posNE; ///< last known local NE position vector (m)
|
||||
Vector2f _last_known_posNE{}; ///< last known local NE position vector (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{0}; ///< last time the accel bias check passed (uSec)
|
||||
uint64_t _delta_time_baro_us{0}; ///< delta time between two consecutive delayed baro samples from the buffer (uSec)
|
||||
|
||||
Vector3f _earth_rate_NED; ///< earth rotation vector (NED) in rad/s
|
||||
Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s
|
||||
|
||||
Dcmf _R_to_earth; ///< transformation matrix from body frame to earth frame from last EKF prediction
|
||||
Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction
|
||||
|
||||
// used by magnetometer fusion mode selection
|
||||
Vector2f _accel_lpf_NE; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
|
||||
Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
|
||||
float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad)
|
||||
float _yaw_rate_lpf_ef{0.0f}; ///< Filtered angular rate about earth frame D axis (rad/sec)
|
||||
bool _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable
|
||||
@@ -392,57 +393,57 @@ private:
|
||||
uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected
|
||||
uint8_t _num_bad_flight_yaw_events{0}; ///< number of times a bad heading has been detected in flight and required a yaw reset
|
||||
uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetometer use was inhibited
|
||||
bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions improve.
|
||||
float _last_static_yaw{0.0f}; ///< last yaw angle recorded when on ground motion checks were passing (rad)
|
||||
|
||||
bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions improve.
|
||||
bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested
|
||||
bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
|
||||
bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements
|
||||
bool _non_mag_yaw_aiding_running_prev{false}; ///< true when heading is being fused from other sources that are not the magnetometer (for example EV or GPS).
|
||||
|
||||
bool _is_yaw_fusion_inhibited{false}; ///< true when yaw sensor use is being inhibited
|
||||
|
||||
SquareMatrix24f P; ///< state covariance matrix
|
||||
SquareMatrix24f P{}; ///< state covariance matrix
|
||||
|
||||
Vector3f _delta_vel_bias_var_accum; ///< kahan summation algorithm accumulator for delta velocity bias variance
|
||||
Vector3f _delta_angle_bias_var_accum; ///< kahan summation algorithm accumulator for delta angle bias variance
|
||||
Vector3f _delta_vel_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta velocity bias variance
|
||||
Vector3f _delta_angle_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta angle bias variance
|
||||
|
||||
Vector3f _last_vel_obs; ///< last velocity observation (m/s)
|
||||
Vector3f _last_vel_obs_var; ///< last velocity observation variance (m/s)**2
|
||||
Vector2f _last_fail_hvel_innov; ///< last failed horizontal velocity innovation (m/s)**2
|
||||
Vector3f _last_vel_obs{}; ///< last velocity observation (m/s)
|
||||
Vector3f _last_vel_obs_var{}; ///< last velocity observation variance (m/s)**2
|
||||
Vector2f _last_fail_hvel_innov{}; ///< last failed horizontal velocity innovation (m/s)**2
|
||||
float _vert_pos_innov_ratio{0.f}; ///< vertical position innovation divided by estimated standard deviation of innovation
|
||||
uint64_t _vert_pos_fuse_attempt_time_us{0}; ///< last system time in usec vertical position measurement fuson was attempted
|
||||
float _vert_vel_innov_ratio{0.f}; ///< standard deviation of vertical velocity innovation
|
||||
uint64_t _vert_vel_fuse_time_us{0}; ///< last system time in usec time vertical velocity measurement fuson was attempted
|
||||
|
||||
Vector3f _gps_vel_innov; ///< GPS velocity innovations (m/sec)
|
||||
Vector3f _gps_vel_innov_var; ///< GPS velocity innovation variances ((m/sec)**2)
|
||||
Vector3f _gps_vel_innov{}; ///< GPS velocity innovations (m/sec)
|
||||
Vector3f _gps_vel_innov_var{}; ///< GPS velocity innovation variances ((m/sec)**2)
|
||||
|
||||
Vector3f _gps_pos_innov; ///< GPS position innovations (m)
|
||||
Vector3f _gps_pos_innov_var; ///< GPS position innovation variances (m**2)
|
||||
Vector3f _gps_pos_innov{}; ///< GPS position innovations (m)
|
||||
Vector3f _gps_pos_innov_var{}; ///< GPS position innovation variances (m**2)
|
||||
|
||||
Vector3f _ev_vel_innov; ///< external vision velocity innovations (m/sec)
|
||||
Vector3f _ev_vel_innov_var; ///< external vision velocity innovation variances ((m/sec)**2)
|
||||
Vector3f _ev_vel_innov{}; ///< external vision velocity innovations (m/sec)
|
||||
Vector3f _ev_vel_innov_var{}; ///< external vision velocity innovation variances ((m/sec)**2)
|
||||
|
||||
Vector3f _ev_pos_innov; ///< external vision position innovations (m)
|
||||
Vector3f _ev_pos_innov_var; ///< external vision position innovation variances (m**2)
|
||||
Vector3f _ev_pos_innov{}; ///< external vision position innovations (m)
|
||||
Vector3f _ev_pos_innov_var{}; ///< external vision position innovation variances (m**2)
|
||||
|
||||
Vector3f _baro_hgt_innov; ///< baro hgt innovations (m)
|
||||
Vector3f _baro_hgt_innov_var; ///< baro hgt innovation variances (m**2)
|
||||
Vector3f _baro_hgt_innov{}; ///< baro hgt innovations (m)
|
||||
Vector3f _baro_hgt_innov_var{}; ///< baro hgt innovation variances (m**2)
|
||||
|
||||
Vector3f _rng_hgt_innov; ///< range hgt innovations (m)
|
||||
Vector3f _rng_hgt_innov_var; ///< range hgt innovation variances (m**2)
|
||||
Vector3f _rng_hgt_innov{}; ///< range hgt innovations (m)
|
||||
Vector3f _rng_hgt_innov_var{}; ///< range hgt innovation variances (m**2)
|
||||
|
||||
Vector3f _aux_vel_innov; ///< horizontal auxiliary velocity innovations: (m/sec)
|
||||
Vector3f _aux_vel_innov_var; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2)
|
||||
Vector3f _aux_vel_innov{}; ///< horizontal auxiliary velocity innovations: (m/sec)
|
||||
Vector3f _aux_vel_innov_var{}; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2)
|
||||
|
||||
float _heading_innov{0.0f}; ///< heading measurement innovation (rad)
|
||||
float _heading_innov_var{0.0f}; ///< heading measurement innovation variance (rad**2)
|
||||
|
||||
Vector3f _mag_innov; ///< earth magnetic field innovations (Gauss)
|
||||
Vector3f _mag_innov_var; ///< earth magnetic field innovation variance (Gauss**2)
|
||||
Vector3f _mag_innov{}; ///< earth magnetic field innovations (Gauss)
|
||||
Vector3f _mag_innov_var{}; ///< earth magnetic field innovation variance (Gauss**2)
|
||||
|
||||
Vector2f _drag_innov; ///< multirotor drag measurement innovation (m/sec**2)
|
||||
Vector2f _drag_innov_var; ///< multirotor drag measurement innovation variance ((m/sec**2)**2)
|
||||
Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2)
|
||||
Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2)
|
||||
|
||||
float _airspeed_innov{0.0f}; ///< airspeed measurement innovation (m/sec)
|
||||
float _airspeed_innov_var{0.0f}; ///< airspeed measurement innovation variance ((m/sec)**2)
|
||||
@@ -454,27 +455,29 @@ private:
|
||||
float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2)
|
||||
|
||||
// optical flow processing
|
||||
Vector2f _flow_innov; ///< flow measurement innovation (rad/sec)
|
||||
Vector2f _flow_innov_var; ///< flow innovation variance ((rad/sec)**2)
|
||||
Vector3f _flow_gyro_bias; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
|
||||
Vector2f _flow_vel_body; ///< velocity from corrected flow measurement (body frame)(m/s)
|
||||
Vector2f _flow_vel_ne; ///< velocity from corrected flow measurement (local frame) (m/s)
|
||||
Vector3f _imu_del_ang_of; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad)
|
||||
Vector2f _flow_innov{}; ///< flow measurement innovation (rad/sec)
|
||||
Vector2f _flow_innov_var{}; ///< flow innovation variance ((rad/sec)**2)
|
||||
Vector3f _flow_gyro_bias{}; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
|
||||
Vector2f _flow_vel_body{}; ///< velocity from corrected flow measurement (body frame)(m/s)
|
||||
Vector2f _flow_vel_ne{}; ///< velocity from corrected flow measurement (local frame) (m/s)
|
||||
Vector3f _imu_del_ang_of{}; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad)
|
||||
|
||||
float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec)
|
||||
uint64_t _time_bad_motion_us{0}; ///< last system time that on-ground motion exceeded limits (uSec)
|
||||
uint64_t _time_good_motion_us{0}; ///< last system time that on-ground motion was within limits (uSec)
|
||||
bool _inhibit_flow_use{false}; ///< true when use of optical flow and range finder is being inhibited
|
||||
Vector2f _flow_compensated_XY_rad; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
|
||||
Vector2f _flow_compensated_XY_rad{}; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
|
||||
|
||||
// output predictor states
|
||||
Vector3f _delta_angle_corr; ///< delta angle correction vector (rad)
|
||||
Vector3f _vel_err_integ; ///< integral of velocity tracking error (m)
|
||||
Vector3f _pos_err_integ; ///< integral of position tracking error (m.s)
|
||||
Vector3f _output_tracking_error; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
|
||||
Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad)
|
||||
Vector3f _vel_err_integ{}; ///< integral of velocity tracking error (m)
|
||||
Vector3f _pos_err_integ{}; ///< integral of position tracking error (m.s)
|
||||
Vector3f _output_tracking_error{}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
|
||||
|
||||
// variables used for the GPS quality checks
|
||||
Vector3f _gps_pos_deriv_filt; ///< GPS NED position derivative (m/sec)
|
||||
Vector2f _gps_velNE_filt; ///< filtered GPS North and East velocity (m/sec)
|
||||
Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec)
|
||||
Vector2f _gps_velNE_filt{}; ///< filtered GPS North and East velocity (m/sec)
|
||||
|
||||
float _gps_velD_diff_filt{0.0f}; ///< GPS filtered Down velocity (m/sec)
|
||||
uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks
|
||||
uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks
|
||||
@@ -503,17 +506,17 @@ private:
|
||||
uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec)
|
||||
uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
|
||||
float _saved_mag_bf_variance[4] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
|
||||
Matrix2f _saved_mag_ef_covmat; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
|
||||
Matrix2f _saved_mag_ef_covmat{}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
|
||||
bool _velpos_reset_request{false}; ///< true when a large yaw error has been fixed and a velocity and position state reset is required
|
||||
|
||||
gps_check_fail_status_u _gps_check_fail_status{};
|
||||
|
||||
// variables used to inhibit accel bias learning
|
||||
bool _accel_bias_inhibit[3] {}; ///< true when the accel bias learning is being inhibited for the specified axis
|
||||
Vector3f _accel_vec_filt; ///< acceleration vector after application of a low pass filter (m/sec**2)
|
||||
Vector3f _accel_vec_filt{}; ///< acceleration vector after application of a low pass filter (m/sec**2)
|
||||
float _accel_magnitude_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
|
||||
float _ang_rate_magnitude_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
|
||||
Vector3f _prev_dvel_bias_var{}; ///< saved delta velocity XYZ bias variances (m/sec)**2
|
||||
|
||||
// Terrain height state estimation
|
||||
float _terrain_vpos{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
|
||||
@@ -978,7 +981,7 @@ private:
|
||||
// Declarations used to control use of the EKF-GSF yaw estimator
|
||||
|
||||
// yaw estimator instance
|
||||
EKFGSF_yaw _yawEstimator;
|
||||
EKFGSF_yaw _yawEstimator{};
|
||||
|
||||
int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec)
|
||||
bool _do_ekfgsf_yaw_reset{false}; // true when an emergency yaw reset has been requested
|
||||
@@ -994,4 +997,5 @@ private:
|
||||
|
||||
void resetGpsDriftCheckFilters();
|
||||
};
|
||||
|
||||
#endif // !EKF_EKF_H
|
||||
|
||||
@@ -264,7 +264,7 @@ protected:
|
||||
|
||||
virtual bool init(uint64_t timestamp) = 0;
|
||||
|
||||
parameters _params; // filter parameters
|
||||
parameters _params{}; // filter parameters
|
||||
|
||||
/*
|
||||
OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer
|
||||
@@ -310,9 +310,9 @@ protected:
|
||||
outputSample _output_new{}; // filter output on the non-delayed time horizon
|
||||
outputVert _output_vert_new{}; // vertical filter output on the non-delayed time horizon
|
||||
imuSample _newest_high_rate_imu_sample{}; // imu sample capturing the newest imu data
|
||||
Matrix3f _R_to_earth_now; // rotation matrix from body to earth frame at current time
|
||||
Vector3f _vel_imu_rel_body_ned; // velocity of IMU relative to body origin in NED earth frame
|
||||
Vector3f _vel_deriv; // velocity derivative at the IMU in NED earth frame (m/s/s)
|
||||
Matrix3f _R_to_earth_now{}; // rotation matrix from body to earth frame at current time
|
||||
Vector3f _vel_imu_rel_body_ned{}; // velocity of IMU relative to body origin in NED earth frame
|
||||
Vector3f _vel_deriv{}; // velocity derivative at the IMU in NED earth frame (m/s/s)
|
||||
|
||||
bool _imu_updated{false}; // true if the ekf should update (completed downsampling process)
|
||||
bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized
|
||||
@@ -328,19 +328,19 @@ protected:
|
||||
|
||||
// innovation consistency check monitoring ratios
|
||||
float _yaw_test_ratio{}; // yaw innovation consistency check ratio
|
||||
Vector3f _mag_test_ratio; // magnetometer XYZ innovation consistency check ratios
|
||||
Vector2f _gps_vel_test_ratio; // GPS velocity innovation consistency check ratios
|
||||
Vector2f _gps_pos_test_ratio; // GPS position innovation consistency check ratios
|
||||
Vector2f _ev_vel_test_ratio; // EV velocity innovation consistency check ratios
|
||||
Vector2f _ev_pos_test_ratio ; // EV position innovation consistency check ratios
|
||||
Vector2f _aux_vel_test_ratio; // Auxiliary horizontal velocity innovation consistency check ratio
|
||||
Vector2f _baro_hgt_test_ratio; // baro height innovation consistency check ratios
|
||||
Vector2f _rng_hgt_test_ratio; // range finder height innovation consistency check ratios
|
||||
Vector3f _mag_test_ratio{}; // magnetometer XYZ innovation consistency check ratios
|
||||
Vector2f _gps_vel_test_ratio{}; // GPS velocity innovation consistency check ratios
|
||||
Vector2f _gps_pos_test_ratio{}; // GPS position innovation consistency check ratios
|
||||
Vector2f _ev_vel_test_ratio{}; // EV velocity innovation consistency check ratios
|
||||
Vector2f _ev_pos_test_ratio{}; // EV position innovation consistency check ratios
|
||||
Vector2f _aux_vel_test_ratio{}; // Auxiliary horizontal velocity innovation consistency check ratio
|
||||
Vector2f _baro_hgt_test_ratio{}; // baro height innovation consistency check ratios
|
||||
Vector2f _rng_hgt_test_ratio{}; // range finder height innovation consistency check ratios
|
||||
float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio
|
||||
float _tas_test_ratio{}; // tas innovation consistency check ratio
|
||||
float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio
|
||||
float _beta_test_ratio{}; // sideslip innovation consistency check ratio
|
||||
Vector2f _drag_test_ratio; // drag innovation consistency check ratio
|
||||
Vector2f _drag_test_ratio{}; // drag innovation consistency check ratio
|
||||
innovation_fault_status_u _innov_check_fail_status{};
|
||||
|
||||
bool _is_dead_reckoning{false}; // true if we are no longer fusing measurements that constrain horizontal velocity drift
|
||||
@@ -420,9 +420,9 @@ private:
|
||||
unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
|
||||
|
||||
// IMU vibration and movement monitoring
|
||||
Vector3f _delta_ang_prev; // delta angle from the previous IMU measurement
|
||||
Vector3f _delta_vel_prev; // delta velocity from the previous IMU measurement
|
||||
Vector3f _vibe_metrics; // IMU vibration metrics
|
||||
Vector3f _delta_ang_prev{}; // delta angle from the previous IMU measurement
|
||||
Vector3f _delta_vel_prev{}; // delta velocity from the previous IMU measurement
|
||||
Vector3f _vibe_metrics{}; // IMU vibration metrics
|
||||
// [0] Level of coning vibration in the IMU delta angles (rad^2)
|
||||
// [1] high frequency vibration level in the IMU delta angle data (rad)
|
||||
// [2] high frequency vibration level in the IMU delta velocity data (m/s)
|
||||
@@ -438,7 +438,7 @@ private:
|
||||
|
||||
// Used to downsample magnetometer data
|
||||
uint64_t _mag_timestamp_sum{0};
|
||||
Vector3f _mag_data_sum;
|
||||
Vector3f _mag_data_sum{};
|
||||
uint8_t _mag_sample_count{0};
|
||||
|
||||
// observation buffer final allocation failed
|
||||
|
||||
@@ -55,9 +55,9 @@ public:
|
||||
void setLimits(float min_distance_m, float max_distance_m);
|
||||
|
||||
private:
|
||||
rangeSample _range_sample {};
|
||||
float _min_distance {0.2f};
|
||||
float _max_distance {20.0f};
|
||||
rangeSample _range_sample{};
|
||||
float _min_distance{0.2f};
|
||||
float _max_distance{20.0f};
|
||||
|
||||
void send(uint64_t time) override;
|
||||
};
|
||||
|
||||
@@ -63,9 +63,9 @@
|
||||
using namespace sensor_simulator::sensor;
|
||||
|
||||
struct sensor_info {
|
||||
uint64_t timestamp {};
|
||||
uint64_t timestamp{};
|
||||
enum measurement_t {IMU, MAG, BARO, GPS, AIRSPEED, RANGE, FLOW, VISION, LANDING_STATUS} sensor_type = IMU;
|
||||
std::array<double, 10> sensor_data {};
|
||||
std::array<double, 10> sensor_data{};
|
||||
};
|
||||
|
||||
class SensorSimulator
|
||||
@@ -120,8 +120,6 @@ public:
|
||||
RangeFinder _rng;
|
||||
Vio _vio;
|
||||
|
||||
protected:
|
||||
|
||||
private:
|
||||
void setSensorDataToDefault();
|
||||
void setSensorDataFromReplayData();
|
||||
@@ -130,14 +128,14 @@ private:
|
||||
void startBasicSensor();
|
||||
void updateSensors();
|
||||
|
||||
std::shared_ptr<Ekf> _ekf {nullptr};
|
||||
std::shared_ptr<Ekf> _ekf{nullptr};
|
||||
|
||||
std::vector<sensor_info> _replay_data {};
|
||||
std::vector<sensor_info> _replay_data{};
|
||||
|
||||
bool _has_replay_data {false};
|
||||
bool _has_replay_data{false};
|
||||
|
||||
uint64_t _current_replay_data_index {0};
|
||||
uint64_t _time {0}; // microseconds
|
||||
uint64_t _current_replay_data_index{0};
|
||||
uint64_t _time{0}; // microseconds
|
||||
|
||||
};
|
||||
#endif // !EKF_SENSOR_SIMULATOR_H
|
||||
|
||||
Reference in New Issue
Block a user