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

ekf2: new kconfig to enable/disable GNSS (enabled by default)

This commit is contained in:
Daniel Agar 2023-10-11 14:02:34 -04:00 committed by GitHub
parent 2d78383296
commit d2b3e7fe16
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
24 changed files with 476 additions and 331 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/bitcraze/crazyflie/default.px4board
View File

@ -15,7 +15,7 @@ CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
# CONFIG_EKF2_GNSS_YAW is not set
# CONFIG_EKF2_GNSS is not set
# CONFIG_EKF2_SIDESLIP is not set
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y

2
boards/bitcraze/crazyflie21/default.px4board
View File

@ -14,7 +14,7 @@ CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
# CONFIG_EKF2_GNSS_YAW is not set
# CONFIG_EKF2_GNSS is not set
# CONFIG_EKF2_MAGNETOMETER is not set
# CONFIG_EKF2_SIDESLIP is not set
CONFIG_MODULES_EVENTS=y

11
src/modules/ekf2/CMakeLists.txt
View File

@ -124,9 +124,6 @@ list(APPEND EKF_SRCS
EKF/estimator_interface.cpp
EKF/fake_height_control.cpp
EKF/fake_pos_control.cpp
EKF/gnss_height_control.cpp
EKF/gps_checks.cpp
EKF/gps_control.cpp
EKF/gravity_fusion.cpp
EKF/height_control.cpp
EKF/imu_down_sampler.cpp
@ -166,6 +163,14 @@ if(CONFIG_EKF2_EXTERNAL_VISION)
)
endif()
if(CONFIG_EKF2_GNSS)
list(APPEND EKF_SRCS
EKF/gnss_height_control.cpp
EKF/gps_checks.cpp
EKF/gps_control.cpp
)
endif()
if(CONFIG_EKF2_GNSS_YAW)
list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
endif()

11
src/modules/ekf2/EKF/CMakeLists.txt
View File

@ -42,9 +42,6 @@ list(APPEND EKF_SRCS
estimator_interface.cpp
fake_height_control.cpp
fake_pos_control.cpp
gnss_height_control.cpp
gps_checks.cpp
gps_control.cpp
gravity_fusion.cpp
height_control.cpp
imu_down_sampler.cpp
@ -84,6 +81,14 @@ if(CONFIG_EKF2_EXTERNAL_VISION)
)
endif()
if(CONFIG_EKF2_GNSS)
list(APPEND EKF_SRCS
gnss_height_control.cpp
gps_checks.cpp
gps_control.cpp
)
endif()
if(CONFIG_EKF2_GNSS_YAW)
list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
endif()

99
src/modules/ekf2/EKF/common.h
View File

@ -280,15 +280,9 @@ struct parameters {
// measurement source control
int32_t height_sensor_ref{HeightSensor::BARO};
int32_t position_sensor_ref{static_cast<int32_t>(PositionSensor::GNSS)};
int32_t baro_ctrl{1};
int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
int32_t sensor_interval_max_ms{10}; ///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
// measurement time delays
float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec)
float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec)
// input noise
float gyro_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec)
float accel_noise{3.5e-1f}; ///< IMU acceleration noise use for covariance prediction (m/sec**2)
@ -308,18 +302,66 @@ struct parameters {
float switch_on_accel_bias{0.2f}; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
float initial_tilt_err{0.1f}; ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
#if defined(CONFIG_EKF2_BAROMETER)
int32_t baro_ctrl{1};
float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec)
float baro_noise{2.0f}; ///< observation noise for barometric height fusion (m)
float baro_bias_nsd{0.13f}; ///< process noise for barometric height bias estimation (m/s/sqrt(Hz))
float baro_innov_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD)
float gnd_effect_deadzone{5.0f}; ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m)
float gnd_effect_max_hgt{0.5f}; ///< Height above ground at which baro ground effect becomes insignificant (m)
# if defined(CONFIG_EKF2_BARO_COMPENSATION)
// static barometer pressure position error coefficient along body axes
float static_pressure_coef_xp{0.0f}; // (-)
float static_pressure_coef_xn{0.0f}; // (-)
float static_pressure_coef_yp{0.0f}; // (-)
float static_pressure_coef_yn{0.0f}; // (-)
float static_pressure_coef_z{0.0f}; // (-)
// upper limit on airspeed used for correction (m/s**2)
float max_correction_airspeed{20.0f};
# endif // CONFIG_EKF2_BARO_COMPENSATION
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_GNSS)
int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec)
Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m)
// position and velocity fusion
float gps_vel_noise{0.5f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec)
float gps_pos_noise{0.5f}; ///< minimum allowed observation noise for gps position fusion (m)
float gps_hgt_bias_nsd{0.13f}; ///< process noise for gnss height bias estimation (m/s/sqrt(Hz))
float pos_noaid_noise{10.0f}; ///< observation noise for non-aiding position fusion (m)
float baro_noise{2.0f}; ///< observation noise for barometric height fusion (m)
float baro_bias_nsd{0.13f}; ///< process noise for barometric height bias estimation (m/s/sqrt(Hz))
float baro_innov_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD)
float gps_pos_innov_gate{5.0f}; ///< GPS horizontal position innovation consistency gate size (STD)
float gps_vel_innov_gate{5.0f}; ///< GPS velocity innovation consistency gate size (STD)
float gnd_effect_deadzone{5.0f}; ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m)
float gnd_effect_max_hgt{0.5f}; ///< Height above ground at which baro ground effect becomes insignificant (m)
// these parameters control the strictness of GPS quality checks used to determine if the GPS is
// good enough to set a local origin and commence aiding
int32_t gps_check_mask{21}; ///< bitmask used to control which GPS quality checks are used
float req_hacc{5.0f}; ///< maximum acceptable horizontal position error (m)
float req_vacc{8.0f}; ///< maximum acceptable vertical position error (m)
float req_sacc{1.0f}; ///< maximum acceptable speed error (m/s)
int32_t req_nsats{6}; ///< minimum acceptable satellite count
float req_pdop{2.0f}; ///< maximum acceptable position dilution of precision
float req_hdrift{0.3f}; ///< maximum acceptable horizontal drift speed (m/s)
float req_vdrift{0.5f}; ///< maximum acceptable vertical drift speed (m/s)
# if defined(CONFIG_EKF2_GNSS_YAW)
// GNSS heading fusion
float gps_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad)
# endif // CONFIG_EKF2_GNSS_YAW
// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
const unsigned EKFGSF_reset_delay{1000000}; ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value
const float EKFGSF_yaw_err_max{0.262f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
#endif // CONFIG_EKF2_GNSS
float pos_noaid_noise{10.0f}; ///< observation noise for non-aiding position fusion (m)
float heading_innov_gate{2.6f}; ///< heading fusion innovation consistency gate size (STD)
float mag_heading_noise{3.0e-1f}; ///< measurement noise used for simple heading fusion (rad)
@ -346,11 +388,6 @@ struct parameters {
float mag_check_inclination_tolerance_deg{20.f};
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_GNSS_YAW)
// GNSS heading fusion
float gps_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad)
#endif // CONFIG_EKF2_GNSS_YAW
#if defined(CONFIG_EKF2_AIRSPEED)
// airspeed fusion
float airspeed_delay_ms{100.0f}; ///< airspeed measurement delay relative to the IMU (mSec)
@ -434,20 +471,8 @@ struct parameters {
Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m)
#endif // CONFIG_EKF2_OPTICAL_FLOW
// these parameters control the strictness of GPS quality checks used to determine if the GPS is
// good enough to set a local origin and commence aiding
int32_t gps_check_mask{21}; ///< bitmask used to control which GPS quality checks are used
float req_hacc{5.0f}; ///< maximum acceptable horizontal position error (m)
float req_vacc{8.0f}; ///< maximum acceptable vertical position error (m)
float req_sacc{1.0f}; ///< maximum acceptable speed error (m/s)
int32_t req_nsats{6}; ///< minimum acceptable satellite count
float req_pdop{2.0f}; ///< maximum acceptable position dilution of precision
float req_hdrift{0.3f}; ///< maximum acceptable horizontal drift speed (m/s)
float req_vdrift{0.5f}; ///< maximum acceptable vertical drift speed (m/s)
// XYZ offset of sensors in body axes (m)
Vector3f imu_pos_body{}; ///< xyz position of IMU in body frame (m)
Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m)
// accel bias learning control
float acc_bias_lim{0.4f}; ///< maximum accel bias magnitude (m/sec**2)
@ -463,18 +488,6 @@ struct parameters {
int32_t valid_timeout_max{5'000'000}; ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
// static barometer pressure position error coefficient along body axes
float static_pressure_coef_xp{0.0f}; // (-)
float static_pressure_coef_xn{0.0f}; // (-)
float static_pressure_coef_yp{0.0f}; // (-)
float static_pressure_coef_yn{0.0f}; // (-)
float static_pressure_coef_z{0.0f}; // (-)
// upper limit on airspeed used for correction (m/s**2)
float max_correction_airspeed {20.0f};
#endif // CONFIG_EKF2_BARO_COMPENSATION
#if defined(CONFIG_EKF2_DRAG_FUSION)
// multi-rotor drag specific force fusion
int32_t drag_ctrl{0};
@ -496,10 +509,6 @@ struct parameters {
const float auxvel_gate{5.0f}; ///< velocity fusion innovation consistency gate size (STD)
#endif // CONFIG_EKF2_AUXVEL
// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
const unsigned EKFGSF_reset_delay{1000000}; ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value
const float EKFGSF_yaw_err_max{0.262f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
};
union fault_status_u {

2
src/modules/ekf2/EKF/control.cpp
View File

@ -111,7 +111,9 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
controlOpticalFlowFusion(imu_delayed);
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_GNSS)
controlGpsFusion(imu_delayed);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_AIRSPEED)
controlAirDataFusion(imu_delayed);

19
src/modules/ekf2/EKF/covariance.cpp
View File

@ -57,16 +57,24 @@ void Ekf::initialiseCovariance()
resetQuatCov();
// velocity
#if defined(CONFIG_EKF2_GNSS)
const float vel_var = sq(fmaxf(_params.gps_vel_noise, 0.01f));
#else
const float vel_var = sq(0.5f);
#endif
P.uncorrelateCovarianceSetVariance<State::vel.dof>(State::vel.idx, Vector3f(vel_var, vel_var, sq(1.5f) * vel_var));
// position
const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f));
float z_pos_var = sq(fmaxf(_params.baro_noise, 0.01f));
#if defined(CONFIG_EKF2_GNSS)
const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f));
if (_control_status.flags.gps_hgt) {
z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
}
#else
const float xy_pos_var = sq(fmaxf(_params.pos_noaid_noise, 0.01f));
#endif
#if defined(CONFIG_EKF2_RANGE_FINDER)
if (_control_status.flags.rng_hgt) {
@ -404,8 +412,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
// check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
bool bad_acc_bias = false;
if (fabsf(down_dvel_bias) > dVel_bias_lim) {
#if defined(CONFIG_EKF2_GNSS)
bool bad_vz_gps = _control_status.flags.gps && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f);
#else
bool bad_vz_gps = false;
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
bool bad_vz_ev = _control_status.flags.ev_vel && (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.0f);
#else
@ -418,7 +429,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
#else
bool bad_z_baro = false;
#endif
#if defined(CONFIG_EKF2_GNSS)
bool bad_z_gps = _control_status.flags.gps_hgt && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f);
#else
bool bad_z_gps = false;
#endif
#if defined(CONFIG_EKF2_RANGE_FINDER)
bool bad_z_rng = _control_status.flags.rng_hgt && (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.0f);

9
src/modules/ekf2/EKF/ekf.cpp
View File

@ -92,7 +92,11 @@ void Ekf::reset()
_prev_gyro_bias_var.zero();
_prev_accel_bias_var.zero();
#if defined(CONFIG_EKF2_GNSS)
resetGpsDriftCheckFilters();
_gps_checks_passed = false;
#endif // CONFIG_EKF2_GNSS
_gps_alt_ref = NAN;
_output_predictor.reset();
@ -112,9 +116,6 @@ void Ekf::reset()
_time_acc_bias_check = 0;
_gps_checks_passed = false;
_gps_alt_ref = NAN;
#if defined(CONFIG_EKF2_BAROMETER)
_baro_counter = 0;
#endif // CONFIG_EKF2_BAROMETER
@ -147,6 +148,7 @@ void Ekf::reset()
resetEstimatorAidStatus(_aid_src_ev_yaw);
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GNSS)
resetEstimatorAidStatus(_aid_src_gnss_hgt);
resetEstimatorAidStatus(_aid_src_gnss_pos);
resetEstimatorAidStatus(_aid_src_gnss_vel);
@ -154,6 +156,7 @@ void Ekf::reset()
# if defined(CONFIG_EKF2_GNSS_YAW)
resetEstimatorAidStatus(_aid_src_gnss_yaw);
# endif // CONFIG_EKF2_GNSS_YAW
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
resetEstimatorAidStatus(_aid_src_mag_heading);

174
src/modules/ekf2/EKF/ekf.h
View File

@ -80,10 +80,6 @@ public:
static uint8_t getNumberOfStates() { return State::size; }
void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
@ -343,19 +339,11 @@ public:
Vector3f getVelocityVariance() const { return getStateVariance<State::vel>(); };
Vector3f getPositionVariance() const { return getStateVariance<State::pos>(); }
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
bool collect_gps(const gpsMessage &gps) override;
// get the ekf WGS-84 origin position and height and the system time it was last set
// return true if the origin is valid
bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
bool setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude);
float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; }
bool setEkfGlobalOriginAltitude(const float altitude);
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
@ -373,14 +361,6 @@ public:
void resetGyroBias();
void resetAccelBias();
// 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
// return true if the origin is set we are not doing unconstrained free inertial navigation
// and have not started using synthetic position observations to constrain drift
@ -504,26 +484,11 @@ public:
Vector3f calcRotVecVariances() const;
float getYawVar() const;
// set minimum continuous period without GPS fail required to mark a healthy GPS status
void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; }
const gps_check_fail_status_u &gps_check_fail_status() const { return _gps_check_fail_status; }
const decltype(gps_check_fail_status_u::flags) &gps_check_fail_status_flags() const { return _gps_check_fail_status.flags; }
bool gps_checks_passed() const { return _gps_checks_passed; };
// get solution data from the EKF-GSF emergency yaw estimator
// returns false when data is not available
bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
// Returns true if the output of the yaw emergency estimator can be used for a reset
bool isYawEmergencyEstimateAvailable() const;
uint8_t getHeightSensorRef() const { return _height_sensor_ref; }
const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); }
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); }
@ -548,6 +513,29 @@ public:
const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; }
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GNSS)
void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
bool collect_gps(const gpsMessage &gps) override;
// set minimum continuous period without GPS fail required to mark a healthy GPS status
void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; }
const gps_check_fail_status_u &gps_check_fail_status() const { return _gps_check_fail_status; }
const decltype(gps_check_fail_status_u::flags) &gps_check_fail_status_flags() const { return _gps_check_fail_status.flags; }
bool gps_checks_passed() const { return _gps_checks_passed; };
// get solution data from the EKF-GSF emergency yaw estimator
// returns false when data is not available
bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); }
const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; }
const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
@ -555,6 +543,7 @@ public:
# if defined(CONFIG_EKF2_GNSS_YAW)
const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
# endif // CONFIG_EKF2_GNSS_YAW
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
@ -606,9 +595,6 @@ private:
bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised
// booleans true when fresh sensor data is available at the fusion time horizon
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
uint64_t _time_last_v_pos_aiding{0};
uint64_t _time_last_v_vel_aiding{0};
@ -710,20 +696,9 @@ private:
uint8_t _nb_ev_yaw_reset_available{0};
#endif // CONFIG_EKF2_EXTERNAL_VISION
estimator_aid_source1d_s _aid_src_gnss_hgt{};
estimator_aid_source2d_s _aid_src_gnss_pos{};
estimator_aid_source3d_s _aid_src_gnss_vel{};
#if defined(CONFIG_EKF2_GNSS_YAW)
estimator_aid_source1d_s _aid_src_gnss_yaw{};
uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
#endif // CONFIG_EKF2_GNSS_YAW
estimator_aid_source3d_s _aid_src_gravity{};
#if defined(CONFIG_EKF2_AUXVEL)
estimator_aid_source2d_s _aid_src_aux_vel{};
#endif // CONFIG_EKF2_AUXVEL
#if defined(CONFIG_EKF2_GNSS)
// booleans true when fresh sensor data is available at the fusion time horizon
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
// variables used for the GPS quality checks
Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec)
@ -736,8 +711,27 @@ private:
uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time
bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed
// Variables used to publish the WGS-84 location of the EKF local NED origin
float _gps_alt_ref{NAN}; ///< WGS-84 height (m)
gps_check_fail_status_u _gps_check_fail_status{};
// height sensor status
bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent
HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
estimator_aid_source1d_s _aid_src_gnss_hgt{};
estimator_aid_source2d_s _aid_src_gnss_pos{};
estimator_aid_source3d_s _aid_src_gnss_vel{};
# if defined(CONFIG_EKF2_GNSS_YAW)
estimator_aid_source1d_s _aid_src_gnss_yaw{};
uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
# endif // CONFIG_EKF2_GNSS_YAW
#endif // CONFIG_EKF2_GNSS
estimator_aid_source3d_s _aid_src_gravity{};
#if defined(CONFIG_EKF2_AUXVEL)
estimator_aid_source2d_s _aid_src_aux_vel{};
#endif // CONFIG_EKF2_AUXVEL
// Variables used by the initial filter alignment
bool _is_first_imu_sample{true};
@ -785,8 +779,6 @@ private:
Matrix3f _saved_mag_bf_covmat{}; ///< magnetic field state covariance sub-matrix that has been saved for use at the next initialisation (Gauss**2)
#endif // CONFIG_EKF2_MAGNETOMETER
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
bool _gyro_bias_inhibit[3] {}; ///< true when the gyro bias learning is being inhibited for the specified axis
@ -797,9 +789,6 @@ private:
Vector3f _prev_gyro_bias_var{}; ///< saved gyro XYZ bias variances
Vector3f _prev_accel_bias_var{}; ///< saved accel XYZ bias variances
// height sensor status
bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent
// imu fault status
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)
@ -818,7 +807,8 @@ private:
void predictCovariance(const imuSample &imu_delayed);
template <const IdxDof &S>
void resetStateCovariance(const matrix::SquareMatrix<float, S.dof> &cov) {
void resetStateCovariance(const matrix::SquareMatrix<float, S.dof> &cov)
{
P.uncorrelateCovarianceSetVariance<S.dof>(S.idx, 0.0f);
P.slice<S.dof, S.dof>(S.idx, S.idx) = cov;
}
@ -828,21 +818,6 @@ private:
bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const;
#if defined(CONFIG_EKF2_GNSS_YAW)
void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
// fuse the yaw angle obtained from a dual antenna GPS unit
void fuseGpsYaw();
// reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit
// return true if the reset was successful
bool resetYawToGps(const float gnss_yaw);
void updateGpsYaw(const gpsSample &gps_sample);
#endif // CONFIG_EKF2_GNSS_YAW
void stopGpsYawFusion();
#if defined(CONFIG_EKF2_MAGNETOMETER)
// ekf sequential fusion of magnetometer measurements
bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);
@ -1079,9 +1054,6 @@ private:
// calculate the earth rotation vector from a given latitude
Vector3f calcEarthRateNED(float lat_rad) const;
// return true id the GPS quality is good enough to set an origin and start aiding
bool gps_is_good(const gpsMessage &gps);
// Control the filter fusion modes
void controlFusionModes(const imuSample &imu_delayed);
@ -1102,11 +1074,43 @@ private:
void stopEvYawFusion();
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GNSS)
// control fusion of GPS observations
void controlGpsFusion(const imuSample &imu_delayed);
void stopGpsFusion();
bool shouldResetGpsFusion() const;
bool isYawFailure() const;
// return true id the GPS quality is good enough to set an origin and start aiding
bool gps_is_good(const gpsMessage &gps);
void controlGnssHeightFusion(const gpsSample &gps_sample);
void stopGpsHgtFusion();
// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
// Resets the horizontal velocity and position to the default navigation sensor
// Returns true if the reset was successful
bool resetYawToEKFGSF();
void resetGpsDriftCheckFilters();
# if defined(CONFIG_EKF2_GNSS_YAW)
void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
void stopGpsYawFusion();
// fuse the yaw angle obtained from a dual antenna GPS unit
void fuseGpsYaw();
// reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit
// return true if the reset was successful
bool resetYawToGps(const float gnss_yaw);
void updateGpsYaw(const gpsSample &gps_sample);
# endif // CONFIG_EKF2_GNSS_YAW
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
// control fusion of magnetometer observations
void controlMagFusion();
@ -1165,7 +1169,6 @@ private:
// control for combined height fusion mode (implemented for switching between baro and range height)
void controlHeightFusion(const imuSample &imu_delayed);
void checkHeightSensorRefFallback();
void controlGnssHeightFusion(const gpsSample &gps_sample);
#if defined(CONFIG_EKF2_BAROMETER)
void controlBaroHeightFusion();
@ -1174,8 +1177,6 @@ private:
void updateGroundEffect();
#endif // CONFIG_EKF2_BAROMETER
void stopGpsHgtFusion();
// gravity fusion: heuristically enable / disable gravity fusion
void controlGravityFusion(const imuSample &imu_delayed);
@ -1212,8 +1213,6 @@ private:
return (sensor_timestamp != 0) && (sensor_timestamp + acceptance_interval > _time_latest_us);
}
void stopGpsFusion();
void resetFakePosFusion();
void stopFakePosFusion();
@ -1232,8 +1231,6 @@ private:
uint8_t _height_sensor_ref{HeightSensor::UNKNOWN};
uint8_t _position_sensor_ref{static_cast<uint8_t>(PositionSensor::GNSS)};
HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
PositionBiasEstimator _ev_pos_b_est{static_cast<uint8_t>(PositionSensor::EV), _position_sensor_ref};
@ -1241,13 +1238,6 @@ private:
bool _ev_q_error_initialized{false};
#endif // CONFIG_EKF2_EXTERNAL_VISION
// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
// Resets the horizontal velocity and position to the default navigation sensor
// Returns true if the reset was successful
bool resetYawToEKFGSF();
void resetGpsDriftCheckFilters();
void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const
{
// only bother resetting if timestamp_sample is set

41
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -206,7 +206,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GNSS)
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_RANGE_FINDER)
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
#endif // CONFIG_EKF2_RANGE_FINDER
@ -293,6 +295,7 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const
-CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad));
}
#if defined(CONFIG_EKF2_GNSS)
void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
{
hvel[0] = _aid_src_gnss_vel.innovation[0];
@ -323,6 +326,7 @@ void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &v
hpos = fmaxf(_aid_src_gnss_pos.test_ratio[0], _aid_src_gnss_pos.test_ratio[1]);
vpos = _aid_src_gnss_hgt.test_ratio;
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
@ -440,11 +444,15 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
// determine current z
float current_alt = -_state.pos(2) + gps_alt_ref_prev;
#if defined(CONFIG_EKF2_GNSS)
const float gps_hgt_bias = _gps_hgt_b_est.getBias();
#endif // CONFIG_EKF2_GNSS
resetVerticalPositionTo(_gps_alt_ref - current_alt);
#if defined(CONFIG_EKF2_GNSS)
// preserve GPS height bias
_gps_hgt_b_est.setBias(gps_hgt_bias);
#endif // CONFIG_EKF2_GNSS
}
return true;
@ -465,9 +473,11 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting is overly optimistic in these situations
if (_control_status.flags.inertial_dead_reckoning) {
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) {
hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_control_status.flags.ev_pos) {
@ -490,9 +500,11 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
// and using state variances for accuracy reporting is overly optimistic in these situations
if (_horizontal_deadreckon_time_exceeded) {
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) {
hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_control_status.flags.ev_pos) {
@ -523,9 +535,11 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) {
vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_control_status.flags.ev_pos) {
@ -670,6 +684,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
vel = NAN;
pos = NAN;
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) {
float gps_vel = sqrtf(Vector3f(_aid_src_gnss_vel.test_ratio).max());
vel = math::max(gps_vel, FLT_MIN);
@ -677,6 +692,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
float gps_pos = sqrtf(Vector2f(_aid_src_gnss_pos.test_ratio).max());
pos = math::max(gps_pos, FLT_MIN);
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_control_status.flags.ev_vel) {
@ -708,10 +724,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
}
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps_hgt) {
hgt_sum += sqrtf(_aid_src_gnss_hgt.test_ratio);
n_hgt_sources++;
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_RANGE_FINDER)
if (_control_status.flags.rng_hgt) {
@ -795,10 +813,15 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_GNSS)
const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f;
const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f;
soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
#else
(void)mag_innov_good;
#endif // CONFIG_EKF2_GNSS
soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical;
*status = soln_status.value;
}
@ -995,6 +1018,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
_time_last_heading_fuse = _time_delayed_us;
}
#if defined(CONFIG_EKF2_GNSS)
bool Ekf::resetYawToEKFGSF()
{
if (!isYawEmergencyEstimateAvailable()) {
@ -1019,9 +1043,11 @@ bool Ekf::resetYawToEKFGSF()
return true;
}
#endif // CONFIG_EKF2_GNSS
bool Ekf::isYawEmergencyEstimateAvailable() const
{
#if defined(CONFIG_EKF2_GNSS)
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
// data and the yaw estimate has converged
if (!_yawEstimator.isActive()) {
@ -1029,23 +1055,18 @@ bool Ekf::isYawEmergencyEstimateAvailable() const
}
return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max);
#else
return false;
#endif
}
#if defined(CONFIG_EKF2_GNSS)
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
{
return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
}
void Ekf::resetGpsDriftCheckFilters()
{
_gps_velNE_filt.setZero();
_gps_pos_deriv_filt.setZero();
_gps_horizontal_position_drift_rate_m_s = NAN;
_gps_vertical_position_drift_rate_m_s = NAN;
_gps_filtered_horizontal_velocity_m_s = NAN;
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_WIND)
void Ekf::resetWind()

8
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -46,7 +46,9 @@
EstimatorInterface::~EstimatorInterface()
{
#if defined(CONFIG_EKF2_GNSS)
delete _gps_buffer;
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
delete _mag_buffer;
#endif // CONFIG_EKF2_MAGNETOMETER
@ -144,6 +146,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_GNSS)
void EstimatorInterface::setGpsData(const gpsMessage &gps)
{
if (!_initialised) {
@ -222,6 +225,7 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us);
}
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_BAROMETER)
void EstimatorInterface::setBaroData(const baroSample &baro_sample)
@ -585,9 +589,11 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
}
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_GNSS)
if (_params.gnss_ctrl > 0) {
max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
if (_params.flow_ctrl > 0) {
@ -713,9 +719,11 @@ void EstimatorInterface::print_status()
printf("minimum observation interval %d us\n", _min_obs_interval_us);
#if defined(CONFIG_EKF2_GNSS)
if (_gps_buffer) {
printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size());
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (_mag_buffer) {

49
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -81,17 +81,24 @@ using namespace estimator;
class EstimatorInterface
{
public:
void setIMUData(const imuSample &imu_sample);
#if defined(CONFIG_EKF2_GNSS)
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
virtual bool collect_gps(const gpsMessage &gps) = 0;
void setGpsData(const gpsMessage &gps);
void setIMUData(const imuSample &imu_sample);
const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; }
float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; }
float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; }
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
void setMagData(const magSample &mag_sample);
#endif // CONFIG_EKF2_MAGNETOMETER
void setGpsData(const gpsMessage &gps);
#if defined(CONFIG_EKF2_BAROMETER)
void setBaroData(const baroSample &baro_sample);
#endif // CONFIG_EKF2_BAROMETER
@ -299,17 +306,12 @@ public:
const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); }
const uint64_t &time_delayed_us() const { return _time_delayed_us; }
const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
const bool &global_origin_valid() const { return _NED_origin_initialised; }
const MapProjection &global_origin() const { return _pos_ref; }
float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; }
void print_status();
float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; }
float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; }
float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; }
OutputPredictor &output_predictor() { return _output_predictor; };
protected:
@ -345,10 +347,6 @@ protected:
OutputPredictor _output_predictor{};
// measurement samples capturing measurements on the delayed time horizon
gpsSample _gps_sample_delayed{};
#if defined(CONFIG_EKF2_AIRSPEED)
airspeedSample _airspeed_sample_delayed{};
#endif // CONFIG_EKF2_AIRSPEED
@ -381,18 +379,33 @@ protected:
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
// Variables used to publish the WGS-84 location of the EKF local NED origin
bool _NED_origin_initialised{false};
MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin
float _gps_alt_ref{NAN}; ///< WGS-84 height (m)
float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin
float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin
MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin
#if defined(CONFIG_EKF2_GNSS)
RingBuffer<gpsSample> *_gps_buffer{nullptr};
uint64_t _time_last_gps_buffer_push{0};
gpsSample _gps_sample_delayed{};
float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s)
float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s)
float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s)
MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message
float _gps_alt_prev{0.0f}; // height from the previous GPS message (m)
# if defined(CONFIG_EKF2_GNSS_YAW)
float _gps_yaw_offset{0.0f}; // Yaw offset angle for dual GPS antennas used for yaw estimation (radians).
// innovation consistency check monitoring ratios
AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
uint64_t _time_last_gps_yaw_buffer_push{0};
# endif // CONFIG_EKF2_GNSS_YAW
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_DRAG_FUSION)
RingBuffer<dragSample> *_drag_buffer{nullptr};
@ -405,10 +418,6 @@ protected:
bool _vertical_position_deadreckon_time_exceeded{true};
bool _vertical_velocity_deadreckon_time_exceeded{true};
float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s)
float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s)
float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s)
uint64_t _time_last_on_ground_us{0}; ///< last time we were on the ground (uSec)
uint64_t _time_last_in_air{0}; ///< last time we were in air (uSec)
@ -416,8 +425,6 @@ protected:
static constexpr uint8_t kBufferLengthDefault = 12;
RingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
RingBuffer<gpsSample> *_gps_buffer{nullptr};
#if defined(CONFIG_EKF2_MAGNETOMETER)
RingBuffer<magSample> *_mag_buffer{nullptr};
uint64_t _time_last_mag_buffer_push{0};
@ -436,8 +443,6 @@ protected:
#endif // CONFIG_EKF2_AUXVEL
RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
uint64_t _time_last_gps_buffer_push{0};
#if defined(CONFIG_EKF2_BAROMETER)
RingBuffer<baroSample> *_baro_buffer{nullptr};
uint64_t _time_last_baro_buffer_push{0};

2
src/modules/ekf2/EKF/ev_height_control.cpp
View File

@ -76,10 +76,12 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
const float measurement = pos(2) - pos_offset_earth(2);
float measurement_var = math::max(pos_cov(2, 2), sq(_params.ev_pos_noise), sq(0.01f));
#if defined(CONFIG_EKF2_GNSS)
// increase minimum variance if GPS active
if (_control_status.flags.gps_hgt) {
measurement_var = math::max(measurement_var, sq(_params.gps_pos_noise));
}
#endif // CONFIG_EKF2_GNSS
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);

2
src/modules/ekf2/EKF/ev_pos_control.cpp
View File

@ -125,12 +125,14 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
break;
}
#if defined(CONFIG_EKF2_GNSS)
// increase minimum variance if GPS active (position reference)
if (_control_status.flags.gps) {
for (int i = 0; i < 2; i++) {
pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
}
}
#endif // CONFIG_EKF2_GNSS
const Vector2f measurement{pos(0), pos(1)};

2
src/modules/ekf2/EKF/ev_vel_control.cpp
View File

@ -106,12 +106,14 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
break;
}
#if defined(CONFIG_EKF2_GNSS)
// increase minimum variance if GPS active (position reference)
if (_control_status.flags.gps) {
for (int i = 0; i < 2; i++) {
vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise));
}
}
#endif // CONFIG_EKF2_GNSS
const Vector3f measurement{vel};

5
src/modules/ekf2/EKF/ev_yaw_control.cpp
View File

@ -155,10 +155,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
_control_status.flags.ev_yaw = true;
} else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) {
// turn on fusion of external vision yaw measurements and disable all other heading fusion
stopGpsYawFusion();
stopGpsFusion();
// turn on fusion of external vision yaw measurements
ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
// reset yaw to EV

2
src/modules/ekf2/EKF/fake_pos_control.cpp
View File

@ -52,7 +52,7 @@ void Ekf::controlFakePosFusion()
Vector2f obs_var;
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
obs_var(0) = obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise));
obs_var(0) = obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, 1.f));
} else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
// Accelerate tilt fine alignment by fusing more

10
src/modules/ekf2/EKF/gps_checks.cpp
View File

@ -271,3 +271,13 @@ bool Ekf::gps_is_good(const gpsMessage &gps)
// continuous period without fail of x seconds required to return a healthy status
return isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us);
}
void Ekf::resetGpsDriftCheckFilters()
{
_gps_velNE_filt.setZero();
_gps_pos_deriv_filt.setZero();
_gps_horizontal_position_drift_rate_m_s = NAN;
_gps_vertical_position_drift_rate_m_s = NAN;
_gps_filtered_horizontal_velocity_m_s = NAN;
}

8
src/modules/ekf2/EKF/gps_control.cpp
View File

@ -400,12 +400,9 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
stopGpsYawFusion();
}
}
#endif // CONFIG_EKF2_GNSS_YAW
void Ekf::stopGpsYawFusion()
{
#if defined(CONFIG_EKF2_GNSS_YAW)
if (_control_status.flags.gps_yaw) {
_control_status.flags.gps_yaw = false;
@ -421,9 +418,8 @@ void Ekf::stopGpsYawFusion()
ECL_INFO("stopping GPS yaw fusion");
}
}
#endif // CONFIG_EKF2_GNSS_YAW
}
#endif // CONFIG_EKF2_GNSS_YAW
void Ekf::stopGpsFusion()
{
@ -436,5 +432,7 @@ void Ekf::stopGpsFusion()
}
stopGpsHgtFusion();
#if defined(CONFIG_EKF2_GNSS_YAW)
stopGpsYawFusion();
#endif // CONFIG_EKF2_GNSS_YAW
}

4
src/modules/ekf2/EKF/height_control.cpp
View File

@ -47,7 +47,9 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed)
controlBaroHeightFusion();
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_GNSS)
controlGnssHeightFusion(_gps_sample_delayed);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_RANGE_FINDER)
controlRangeHeightFusion();
@ -185,6 +187,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
}
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps_hgt) {
checks[1] = {ReferenceType::GNSS, _aid_src_gnss_hgt.innovation, _aid_src_gnss_hgt.innovation_variance};
}
@ -192,6 +195,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
if (_control_status.flags.gps) {
checks[2] = {ReferenceType::GNSS, _aid_src_gnss_vel.innovation[2], _aid_src_gnss_vel.innovation_variance[2]};
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_RANGE_FINDER)
if (_control_status.flags.rng_hgt) {

20
src/modules/ekf2/EKF/optical_flow_control.cpp
View File

@ -142,15 +142,25 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
// New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon
if (_flow_data_ready) {
// Check if we are in-air and require optical flow to control position drift
bool is_flow_required = _control_status.flags.in_air
&& (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));
#if defined(CONFIG_EKF2_GNSS)
// check if using GPS, but GPS is bad
if (_control_status.flags.gps) {
if (_control_status.flags.in_air && !is_flow_required) {
// Inhibit flow use if motion is un-suitable or we have good quality GPS
// Apply hysteresis to prevent rapid mode switching
const float gps_err_norm_lim = _control_status.flags.opt_flow ? 0.7f : 1.0f;
// Check if we are in-air and require optical flow to control position drift
const bool is_flow_required = _control_status.flags.in_air
&& (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)
|| (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
if (_gps_error_norm > gps_err_norm_lim) {
is_flow_required = true;
}
}
}
#endif // CONFIG_EKF2_GNSS
// inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation
const bool preflight_motion_not_ok = !_control_status.flags.in_air

124
src/modules/ekf2/EKF2.cpp
View File

@ -62,7 +62,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_params(_ekf.getParamHandle()),
_param_ekf2_predict_us(_params->filter_update_interval_us),
_param_ekf2_imu_ctrl(_params->imu_ctrl),
_param_ekf2_gps_delay(_params->gps_delay_ms),
#if defined(CONFIG_EKF2_AUXVEL)
_param_ekf2_avel_delay(_params->auxvel_delay_ms),
#endif // CONFIG_EKF2_AUXVEL
@ -73,9 +72,27 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
#if defined(CONFIG_EKF2_WIND)
_param_ekf2_wind_nsd(_params->wind_vel_nsd),
#endif // CONFIG_EKF2_WIND
_param_ekf2_noaid_noise(_params->pos_noaid_noise),
#if defined(CONFIG_EKF2_GNSS)
_param_ekf2_gps_ctrl(_params->gnss_ctrl),
_param_ekf2_gps_delay(_params->gps_delay_ms),
_param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
_param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
_param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
_param_ekf2_gps_v_noise(_params->gps_vel_noise),
_param_ekf2_gps_p_noise(_params->gps_pos_noise),
_param_ekf2_noaid_noise(_params->pos_noaid_noise),
_param_ekf2_gps_p_gate(_params->gps_pos_innov_gate),
_param_ekf2_gps_v_gate(_params->gps_vel_innov_gate),
_param_ekf2_gps_check(_params->gps_check_mask),
_param_ekf2_req_eph(_params->req_hacc),
_param_ekf2_req_epv(_params->req_vacc),
_param_ekf2_req_sacc(_params->req_sacc),
_param_ekf2_req_nsats(_params->req_nsats),
_param_ekf2_req_pdop(_params->req_pdop),
_param_ekf2_req_hdrift(_params->req_hdrift),
_param_ekf2_req_vdrift(_params->req_vdrift),
_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default),
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_BAROMETER)
_param_ekf2_baro_ctrl(_params->baro_ctrl),
_param_ekf2_baro_delay(_params->baro_delay_ms),
@ -92,8 +109,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_pcoef_z(_params->static_pressure_coef_z),
# endif // CONFIG_EKF2_BARO_COMPENSATION
#endif // CONFIG_EKF2_BAROMETER
_param_ekf2_gps_p_gate(_params->gps_pos_innov_gate),
_param_ekf2_gps_v_gate(_params->gps_vel_innov_gate),
#if defined(CONFIG_EKF2_AIRSPEED)
_param_ekf2_asp_delay(_params->airspeed_delay_ms),
_param_ekf2_tas_gate(_params->tas_innov_gate),
@ -123,16 +138,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg),
_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
#endif // CONFIG_EKF2_MAGNETOMETER
_param_ekf2_gps_check(_params->gps_check_mask),
_param_ekf2_req_eph(_params->req_hacc),
_param_ekf2_req_epv(_params->req_vacc),
_param_ekf2_req_sacc(_params->req_sacc),
_param_ekf2_req_nsats(_params->req_nsats),
_param_ekf2_req_pdop(_params->req_pdop),
_param_ekf2_req_hdrift(_params->req_hdrift),
_param_ekf2_req_vdrift(_params->req_vdrift),
_param_ekf2_hgt_ref(_params->height_sensor_ref),
_param_ekf2_gps_ctrl(_params->gnss_ctrl),
_param_ekf2_noaid_tout(_params->valid_timeout_max),
#if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
_param_ekf2_min_rng(_params->rng_gnd_clearance),
@ -171,7 +177,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_ev_pos_y(_params->ev_pos_body(1)),
_param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
#endif // CONFIG_EKF2_EXTERNAL_VISION
_param_ekf2_grav_noise(_params->gravity_noise),
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
_param_ekf2_of_ctrl(_params->flow_ctrl),
_param_ekf2_of_delay(_params->flow_delay_ms),
@ -187,9 +192,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_imu_pos_x(_params->imu_pos_body(0)),
_param_ekf2_imu_pos_y(_params->imu_pos_body(1)),
_param_ekf2_imu_pos_z(_params->imu_pos_body(2)),
_param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
_param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
_param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
_param_ekf2_gbias_init(_params->switch_on_gyro_bias),
_param_ekf2_abias_init(_params->switch_on_accel_bias),
_param_ekf2_angerr_init(_params->initial_tilt_err),
@ -205,7 +207,8 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_bcoef_y(_params->bcoef_y),
_param_ekf2_mcoef(_params->mcoef),
#endif // CONFIG_EKF2_DRAG_FUSION
_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
_param_ekf2_grav_noise(_params->gravity_noise)
{
// advertise expected minimal topic set immediately to ensure logging
_attitude_pub.advertise();
@ -233,7 +236,9 @@ EKF2::~EKF2()
#if defined(CONFIG_EKF2_RANGE_FINDER)
perf_free(_msg_missed_distance_sensor_perf);
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_GNSS)
perf_free(_msg_missed_gps_perf);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_AUXVEL)
perf_free(_msg_missed_landing_target_pose_perf);
#endif // CONFIG_EKF2_AUXVEL
@ -262,7 +267,9 @@ bool EKF2::multi_init(int imu, int mag)
_estimator_states_pub.advertise();
_estimator_status_flags_pub.advertise();
_estimator_status_pub.advertise();
#if defined(CONFIG_EKF2_GNSS)
_yaw_est_pub.advertise();
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_BAROMETER)
@ -297,6 +304,8 @@ bool EKF2::multi_init(int imu, int mag)
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GNSS)
// GNSS advertise
if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::VPOS)) {
_estimator_aid_src_gnss_hgt_pub.advertise();
@ -319,6 +328,7 @@ bool EKF2::multi_init(int imu, int mag)
}
# endif // CONFIG_EKF2_GNSS_YAW
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_RANGE_FINDER)
@ -395,7 +405,9 @@ int EKF2::print_status()
#if defined(CONFIG_EKF2_RANGE_FINDER)
perf_print_counter(_msg_missed_distance_sensor_perf);
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_GNSS)
perf_print_counter(_msg_missed_gps_perf);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_AUXVEL)
perf_print_counter(_msg_missed_landing_target_pose_perf);
#endif // CONFIG_EKF2_AUXVEL
@ -434,7 +446,9 @@ void EKF2::Run()
VerifyParams();
#if defined(CONFIG_EKF2_GNSS)
_ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);
#endif // CONFIG_EKF2_GNSS
const matrix::Vector3f imu_pos_body(_param_ekf2_imu_pos_x.get(),
_param_ekf2_imu_pos_y.get(),
@ -721,7 +735,9 @@ void EKF2::Run()
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
UpdateFlowSample(ekf2_timestamps);
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_GNSS)
UpdateGpsSample(ekf2_timestamps);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
UpdateMagSample(ekf2_timestamps);
#endif // CONFIG_EKF2_MAGNETOMETER
@ -739,43 +755,49 @@ void EKF2::Run()
PublishLocalPosition(now);
PublishOdometry(now, imu_sample_new);
PublishGlobalPosition(now);
PublishSensorBias(now);
#if defined(CONFIG_EKF2_WIND)
PublishWindEstimate(now);
#endif // CONFIG_EKF2_WIND
// publish status/logging messages
#if defined(CONFIG_EKF2_BAROMETER)
PublishBaroBias(now);
#endif // CONFIG_EKF2_BAROMETER
PublishGnssHgtBias(now);
#if defined(CONFIG_EKF2_RANGE_FINDER)
PublishRngHgtBias(now);
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
PublishEvPosBias(now);
#endif // CONFIG_EKF2_EXTERNAL_VISION
PublishEventFlags(now);
PublishGpsStatus(now);
PublishInnovations(now);
PublishInnovationTestRatios(now);
PublishInnovationVariances(now);
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
PublishOpticalFlowVel(now);
#endif // CONFIG_EKF2_OPTICAL_FLOW
PublishStates(now);
PublishStatus(now);
PublishStatusFlags(now);
PublishAidSourceStatus(now);
#if defined(CONFIG_EKF2_BAROMETER)
PublishBaroBias(now);
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_RANGE_FINDER)
PublishRngHgtBias(now);
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
PublishEvPosBias(now);
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GNSS)
PublishGnssHgtBias(now);
PublishGpsStatus(now);
PublishYawEstimatorStatus(now);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
PublishOpticalFlowVel(now);
#endif // CONFIG_EKF2_OPTICAL_FLOW
UpdateAccelCalibration(now);
UpdateGyroCalibration(now);
#if defined(CONFIG_EKF2_MAGNETOMETER)
UpdateMagCalibration(now);
#endif // CONFIG_EKF2_MAGNETOMETER
PublishSensorBias(now);
PublishAidSourceStatus(now);
} else {
// ekf no update
@ -792,6 +814,8 @@ void EKF2::Run()
void EKF2::VerifyParams()
{
#if defined(CONFIG_EKF2_GNSS)
if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS)
|| (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS_YAW)) {
_param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_GPS |
@ -816,6 +840,8 @@ void EKF2::VerifyParams()
"GPS lon/lat is required for altitude fusion", _param_ekf2_gps_ctrl.get());
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_BAROMETER)
if ((_param_ekf2_hgt_ref.get() == HeightSensor::BARO) && (_param_ekf2_baro_ctrl.get() == 0)) {
@ -846,6 +872,8 @@ void EKF2::VerifyParams()
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_GNSS)
if ((_param_ekf2_hgt_ref.get() == HeightSensor::GNSS) && !(_param_ekf2_gps_ctrl.get() & GnssCtrl::VPOS)) {
_param_ekf2_gps_ctrl.set(_param_ekf2_gps_ctrl.get() | (GnssCtrl::VPOS | GnssCtrl::HPOS | GnssCtrl::VEL));
_param_ekf2_gps_ctrl.commit();
@ -857,6 +885,8 @@ void EKF2::VerifyParams()
"GPS enabled by EKF2_HGT_REF", _param_ekf2_gps_ctrl.get());
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if ((_param_ekf2_hgt_ref.get() == HeightSensor::EV)
@ -1034,6 +1064,7 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
PublishAidSourceStatus(_ekf.aid_src_ev_yaw(), _status_ev_yaw_pub_last, _estimator_aid_src_ev_yaw_pub);
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GNSS)
// GNSS hgt/pos/vel/yaw
PublishAidSourceStatus(_ekf.aid_src_gnss_hgt(), _status_gnss_hgt_pub_last, _estimator_aid_src_gnss_hgt_pub);
PublishAidSourceStatus(_ekf.aid_src_gnss_pos(), _status_gnss_pos_pub_last, _estimator_aid_src_gnss_pos_pub);
@ -1041,6 +1072,7 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
# if defined(CONFIG_EKF2_GNSS_YAW)
PublishAidSourceStatus(_ekf.aid_src_gnss_yaw(), _status_gnss_yaw_pub_last, _estimator_aid_src_gnss_yaw_pub);
# endif // CONFIG_EKF2_GNSS_YAW
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
// mag heading
@ -1115,6 +1147,7 @@ void EKF2::PublishBaroBias(const hrt_abstime &timestamp)
}
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_GNSS)
void EKF2::PublishGnssHgtBias(const hrt_abstime &timestamp)
{
if (_ekf.get_gps_sample_delayed().time_us != 0) {
@ -1127,6 +1160,7 @@ void EKF2::PublishGnssHgtBias(const hrt_abstime &timestamp)
}
}
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_RANGE_FINDER)
void EKF2::PublishRngHgtBias(const hrt_abstime &timestamp)
@ -1280,7 +1314,11 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
_ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon);
global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters
#if defined(CONFIG_EKF2_GNSS)
global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt);
#else
global_pos.alt_ellipsoid = global_pos.alt;
#endif
// delta_alt, alt_reset_counter
// global altitude has opposite sign of local down position
@ -1319,6 +1357,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
}
}
#if defined(CONFIG_EKF2_GNSS)
void EKF2::PublishGpsStatus(const hrt_abstime &timestamp)
{
const hrt_abstime timestamp_sample = _ekf.get_gps_sample_delayed().time_us;
@ -1353,13 +1392,16 @@ void EKF2::PublishGpsStatus(const hrt_abstime &timestamp)
_last_gps_status_published = timestamp_sample;
}
#endif // CONFIG_EKF2_GNSS
void EKF2::PublishInnovations(const hrt_abstime &timestamp)
{
// publish estimator innovation data
estimator_innovations_s innovations{};
innovations.timestamp_sample = _ekf.time_delayed_us();
#if defined(CONFIG_EKF2_GNSS)
_ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
_ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos);
#endif // CONFIG_EKF2_EXTERNAL_VISION
@ -1449,8 +1491,10 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
// publish estimator innovation test ratio data
estimator_innovations_s test_ratios{};
test_ratios.timestamp_sample = _ekf.time_delayed_us();
#if defined(CONFIG_EKF2_GNSS)
_ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0],
test_ratios.gps_vpos);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
_ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos);
#endif // CONFIG_EKF2_EXTERNAL_VISION
@ -1504,7 +1548,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
// publish estimator innovation variance data
estimator_innovations_s variances{};
variances.timestamp_sample = _ekf.time_delayed_us();
#if defined(CONFIG_EKF2_GNSS)
_ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
_ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos);
#endif // CONFIG_EKF2_EXTERNAL_VISION
@ -1787,9 +1833,11 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
_ekf.getOutputTrackingError().copyTo(status.output_tracking_error);
#if defined(CONFIG_EKF2_GNSS)
// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
// the GPS Fix bit, which is always checked)
status.gps_check_fail_flags = _ekf.gps_check_fail_status().value & (((uint16_t)_params->gps_check_mask << 1) | 1);
#endif // CONFIG_EKF2_GNSS
status.control_mode_flags = _ekf.control_status().value;
status.filter_fault_flags = _ekf.fault_status().value;
@ -1948,6 +1996,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
}
}
#if defined(CONFIG_EKF2_GNSS)
void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
{
static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF,
@ -1967,6 +2016,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
_yaw_est_pub.publish(yaw_est_test_data);
}
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_WIND)
void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
@ -2029,6 +2079,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_GNSS)
float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
{
float height_diff = static_cast<float>(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt;
@ -2049,6 +2100,7 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
return amsl_hgt + _wgs84_hgt_offset;
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_AIRSPEED)
void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
@ -2412,6 +2464,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_GNSS)
void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF GPS message
@ -2455,6 +2508,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
_gps_alttitude_ellipsoid = static_cast<int32_t>(round(vehicle_gps_position.altitude_ellipsoid_m * 1e3));
}
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)

167
src/modules/ekf2/EKF2.hpp
View File

@ -68,7 +68,6 @@
#include <uORB/topics/estimator_bias.h>
#include <uORB/topics/estimator_bias3d.h>
#include <uORB/topics/estimator_event_flags.h>
#include <uORB/topics/estimator_gps_status.h>
#include <uORB/topics/estimator_innovations.h>
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/estimator_states.h>
@ -80,7 +79,6 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
@ -101,6 +99,11 @@
# include <uORB/topics/vehicle_air_data.h>
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_GNSS)
# include <uORB/topics/estimator_gps_status.h>
# include <uORB/topics/sensor_gps.h>
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
# include <uORB/topics/vehicle_magnetometer.h>
#endif // CONFIG_EKF2_MAGNETOMETER
@ -180,7 +183,6 @@ private:
#if defined(CONFIG_EKF2_BAROMETER)
void PublishBaroBias(const hrt_abstime &timestamp);
#endif // CONFIG_EKF2_BAROMETER
void PublishGnssHgtBias(const hrt_abstime &timestamp);
#if defined(CONFIG_EKF2_RANGE_FINDER)
void PublishRngHgtBias(const hrt_abstime &timestamp);
@ -193,15 +195,11 @@ private:
uint64_t timestamp, uint32_t device_id = 0);
void PublishEventFlags(const hrt_abstime &timestamp);
void PublishGlobalPosition(const hrt_abstime &timestamp);
void PublishGpsStatus(const hrt_abstime &timestamp);
void PublishInnovations(const hrt_abstime &timestamp);
void PublishInnovationTestRatios(const hrt_abstime &timestamp);
void PublishInnovationVariances(const hrt_abstime &timestamp);
void PublishLocalPosition(const hrt_abstime &timestamp);
void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sample);
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
void PublishOpticalFlowVel(const hrt_abstime &timestamp);
#endif // CONFIG_EKF2_OPTICAL_FLOW
void PublishSensorBias(const hrt_abstime &timestamp);
void PublishStates(const hrt_abstime &timestamp);
void PublishStatus(const hrt_abstime &timestamp);
@ -209,7 +207,6 @@ private:
#if defined(CONFIG_EKF2_WIND)
void PublishWindEstimate(const hrt_abstime &timestamp);
#endif // CONFIG_EKF2_WIND
void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
#if defined(CONFIG_EKF2_AIRSPEED)
void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
@ -223,16 +220,28 @@ private:
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps);
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GNSS)
/*
* Calculate filtered WGS84 height from estimated AMSL height
*/
float filter_altitude_ellipsoid(float amsl_hgt);
void PublishGpsStatus(const hrt_abstime &timestamp);
void PublishGnssHgtBias(const hrt_abstime &timestamp);
void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps);
void PublishOpticalFlowVel(const hrt_abstime &timestamp);
#endif // CONFIG_EKF2_OPTICAL_FLOW
void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
#if defined(CONFIG_EKF2_MAGNETOMETER)
void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_RANGE_FINDER)
void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
#endif // CONFIG_EKF2_RANGE_FINDER
void UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps);
// Used to check, save and use learned accel/gyro/mag biases
@ -267,11 +276,6 @@ private:
}
}
/*
* Calculate filtered WGS84 height from estimated AMSL height
*/
float filter_altitude_ellipsoid(float amsl_hgt);
static constexpr float sq(float x) { return x * x; };
const bool _replay_mode{false}; ///< true when we use replay data from a log
@ -288,17 +292,11 @@ private:
perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")};
perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")};
perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
perf_counter_t _msg_missed_gps_perf{nullptr};
perf_counter_t _msg_missed_odometry_perf{nullptr};
InFlightCalibration _accel_cal{};
InFlightCalibration _gyro_cal{};
uint64_t _gps_time_usec{0};
int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid
uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt
float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84
uint8_t _accel_calibration_count{0};
uint8_t _gyro_calibration_count{0};
@ -309,7 +307,6 @@ private:
Vector3f _last_gyro_bias_published{};
hrt_abstime _last_sensor_bias_published{0};
hrt_abstime _last_gps_status_published{0};
hrt_abstime _status_fake_hgt_pub_last{0};
hrt_abstime _status_fake_pos_pub_last{0};
@ -352,13 +349,6 @@ private:
uORB::PublicationMulti<estimator_bias3d_s> _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_bias)};
#endif // CONFIG_EKF2_EXTERNAL_VISION
hrt_abstime _status_gnss_hgt_pub_last{0};
hrt_abstime _status_gnss_pos_pub_last{0};
hrt_abstime _status_gnss_vel_pub_last{0};
#if defined(CONFIG_EKF2_GNSS_YAW)
hrt_abstime _status_gnss_yaw_pub_last {0};
#endif // CONFIG_EKF2_GNSS_YAW
hrt_abstime _status_gravity_pub_last{0};
#if defined(CONFIG_EKF2_AUXVEL)
@ -413,8 +403,6 @@ private:
hrt_abstime _status_drag_pub_last{0};
#endif // CONFIG_EKF2_DRAG_FUSION
float _last_gnss_hgt_bias_published{};
#if defined(CONFIG_EKF2_AIRSPEED)
uORB::Subscription _airspeed_sub {ORB_ID(airspeed)};
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
@ -441,7 +429,6 @@ private:
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
@ -477,9 +464,7 @@ private:
uint32_t _filter_information_event_changes{0};
uORB::PublicationMulti<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
uORB::PublicationMulti<estimator_bias_s> _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)};
uORB::PublicationMultiData<estimator_event_flags_s> _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
uORB::PublicationMulti<estimator_gps_status_s> _estimator_gps_status_pub{ORB_ID(estimator_gps_status)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
@ -487,18 +472,10 @@ private:
uORB::PublicationMulti<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
uORB::PublicationMulti<estimator_status_flags_s> _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
uORB::PublicationMulti<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
#if defined(CONFIG_EKF2_GNSS_YAW)
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)};
#endif // CONFIG_EKF2_GNSS_YAW
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)};
// publications with topic dependent on multi-mode
@ -511,6 +488,36 @@ private:
uORB::PublicationMulti<wind_s> _wind_pub;
#endif // CONFIG_EKF2_WIND
#if defined(CONFIG_EKF2_GNSS)
perf_counter_t _msg_missed_gps_perf {nullptr};
uint64_t _gps_time_usec{0};
int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid
uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt
float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84
hrt_abstime _last_gps_status_published{0};
hrt_abstime _status_gnss_hgt_pub_last{0};
hrt_abstime _status_gnss_pos_pub_last{0};
hrt_abstime _status_gnss_vel_pub_last{0};
float _last_gnss_hgt_bias_published{};
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::PublicationMulti<estimator_bias_s> _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)};
uORB::PublicationMulti<estimator_gps_status_s> _estimator_gps_status_pub{ORB_ID(estimator_gps_status)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)};
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
# if defined(CONFIG_EKF2_GNSS_YAW)
hrt_abstime _status_gnss_yaw_pub_last {0};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)};
# endif // CONFIG_EKF2_GNSS_YAW
#endif // CONFIG_EKF2_GNSS
PreFlightChecker _preflt_checker;
Ekf _ekf;
@ -521,9 +528,6 @@ private:
(ParamExtInt<px4::params::EKF2_PREDICT_US>) _param_ekf2_predict_us,
(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
(ParamExtFloat<px4::params::EKF2_GPS_DELAY>)
_param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec)
#if defined(CONFIG_EKF2_AUXVEL)
(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
_param_ekf2_avel_delay, ///< auxiliary velocity measurement delay relative to the IMU (mSec)
@ -544,12 +548,35 @@ private:
(ParamExtFloat<px4::params::EKF2_WIND_NSD>) _param_ekf2_wind_nsd,
#endif // CONFIG_EKF2_WIND
(ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>)
_param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec)
(ParamExtFloat<px4::params::EKF2_GPS_P_NOISE>)
_param_ekf2_gps_p_noise, ///< minimum allowed observation noise for gps position fusion (m)
(ParamExtFloat<px4::params::EKF2_NOAID_NOISE>)
_param_ekf2_noaid_noise, ///< observation noise for non-aiding position fusion (m)
(ParamExtFloat<px4::params::EKF2_NOAID_NOISE>) _param_ekf2_noaid_noise,
#if defined(CONFIG_EKF2_GNSS)
(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl,
(ParamExtFloat<px4::params::EKF2_GPS_DELAY>) _param_ekf2_gps_delay,
(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x,
(ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y,
(ParamExtFloat<px4::params::EKF2_GPS_POS_Z>) _param_ekf2_gps_pos_z,
(ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>) _param_ekf2_gps_v_noise,
(ParamExtFloat<px4::params::EKF2_GPS_P_NOISE>) _param_ekf2_gps_p_noise,
(ParamExtFloat<px4::params::EKF2_GPS_P_GATE>) _param_ekf2_gps_p_gate,
(ParamExtFloat<px4::params::EKF2_GPS_V_GATE>) _param_ekf2_gps_v_gate,
(ParamExtInt<px4::params::EKF2_GPS_CHECK>) _param_ekf2_gps_check,
(ParamExtFloat<px4::params::EKF2_REQ_EPH>) _param_ekf2_req_eph,
(ParamExtFloat<px4::params::EKF2_REQ_EPV>) _param_ekf2_req_epv,
(ParamExtFloat<px4::params::EKF2_REQ_SACC>) _param_ekf2_req_sacc,
(ParamExtInt<px4::params::EKF2_REQ_NSATS>) _param_ekf2_req_nsats,
(ParamExtFloat<px4::params::EKF2_REQ_PDOP>) _param_ekf2_req_pdop,
(ParamExtFloat<px4::params::EKF2_REQ_HDRIFT>) _param_ekf2_req_hdrift,
(ParamExtFloat<px4::params::EKF2_REQ_VDRIFT>) _param_ekf2_req_vdrift,
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h,
// Used by EKF-GSF experimental yaw estimator
(ParamExtFloat<px4::params::EKF2_GSF_TAS>) _param_ekf2_gsf_tas_default,
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_BAROMETER)
(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,///< barometer control selection
@ -570,11 +597,6 @@ private:
# endif // CONFIG_EKF2_BARO_COMPENSATION
#endif // CONFIG_EKF2_BAROMETER
(ParamExtFloat<px4::params::EKF2_GPS_P_GATE>)
_param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_GPS_V_GATE>)
_param_ekf2_gps_v_gate, ///< GPS velocity innovation consistency gate size (STD)
#if defined(CONFIG_EKF2_AIRSPEED)
(ParamExtFloat<px4::params::EKF2_ASP_DELAY>)
_param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec)
@ -616,23 +638,10 @@ private:
(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>) _param_ekf2_synthetic_mag_z,
#endif // CONFIG_EKF2_MAGNETOMETER
(ParamExtInt<px4::params::EKF2_GPS_CHECK>)
_param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used
(ParamExtFloat<px4::params::EKF2_REQ_EPH>) _param_ekf2_req_eph, ///< maximum acceptable horiz position error (m)
(ParamExtFloat<px4::params::EKF2_REQ_EPV>) _param_ekf2_req_epv, ///< maximum acceptable vert position error (m)
(ParamExtFloat<px4::params::EKF2_REQ_SACC>) _param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s)
(ParamExtInt<px4::params::EKF2_REQ_NSATS>) _param_ekf2_req_nsats, ///< minimum acceptable satellite count
(ParamExtFloat<px4::params::EKF2_REQ_PDOP>)
_param_ekf2_req_pdop, ///< maximum acceptable position dilution of precision
(ParamExtFloat<px4::params::EKF2_REQ_HDRIFT>)
_param_ekf2_req_hdrift, ///< maximum acceptable horizontal drift speed (m/s)
(ParamExtFloat<px4::params::EKF2_REQ_VDRIFT>) _param_ekf2_req_vdrift, ///< maximum acceptable vertical drift speed (m/s)
// measurement source control
(ParamInt<px4::params::EKF2_AID_MASK>)
_param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
(ParamExtInt<px4::params::EKF2_HGT_REF>) _param_ekf2_hgt_ref, ///< selects the primary source for height data
(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl, ///< GPS control selection
(ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
_param_ekf2_noaid_tout, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec)
@ -689,10 +698,6 @@ private:
(ParamExtFloat<px4::params::EKF2_EV_POS_Z>)
_param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m)
#endif // CONFIG_EKF2_EXTERNAL_VISION
(ParamExtFloat<px4::params::EKF2_GRAV_NOISE>)
_param_ekf2_grav_noise, ///< default accelerometer noise for gravity fusion measurements (m/s**2)
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
// optical flow fusion
(ParamExtInt<px4::params::EKF2_OF_CTRL>)
@ -721,15 +726,6 @@ private:
(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_IMU_POS_Z>) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m)
(ParamExtFloat<px4::params::EKF2_GPS_POS_Z>) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m)
// output predictor filter time constants
(ParamFloat<px4::params::EKF2_TAU_VEL>)
_param_ekf2_tau_vel, ///< time constant used by the output velocity complementary filter (sec)
(ParamFloat<px4::params::EKF2_TAU_POS>)
_param_ekf2_tau_pos, ///< time constant used by the output position complementary filter (sec)
// IMU switch on bias parameters
(ParamExtFloat<px4::params::EKF2_GBIAS_INIT>)
@ -760,12 +756,11 @@ private:
(ParamExtFloat<px4::params::EKF2_MCOEF>) _param_ekf2_mcoef, ///< propeller momentum drag coefficient (1/s)
#endif // CONFIG_EKF2_DRAG_FUSION
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
// Used by EKF-GSF experimental yaw estimator
(ParamExtFloat<px4::params::EKF2_GSF_TAS>)
_param_ekf2_gsf_tas_default ///< default value of true airspeed assumed during fixed wing operation
// output predictor filter time constants
(ParamFloat<px4::params::EKF2_TAU_VEL>) _param_ekf2_tau_vel,
(ParamFloat<px4::params::EKF2_TAU_POS>) _param_ekf2_tau_pos,
(ParamExtFloat<px4::params::EKF2_GRAV_NOISE>) _param_ekf2_grav_noise
)
};
#endif // !EKF2_HPP

8
src/modules/ekf2/Kconfig
View File

@ -60,10 +60,18 @@ depends on MODULES_EKF2
---help---
EKF2 external vision (EV) fusion support.
menuconfig EKF2_GNSS
depends on MODULES_EKF2
bool "GNSS fusion support"
default y
---help---
EKF2 GNSS fusion support.
menuconfig EKF2_GNSS_YAW
depends on MODULES_EKF2
bool "GNSS yaw fusion support"
default y
depends on EKF2_GNSS
---help---
EKF2 GNSS yaw fusion support.