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

ekf2: add kconfig option to enable/disable airspeed and sideslip fusion

Browse Source
This commit is contained in:
Daniel Agar
2023-03-20 10:12:17 -04:00
parent 98ff1afc19
commit 4270a303ab
16 changed files with 218 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/bitcraze/crazyflie/default.px4board
+2
View File
@@ -15,6 +15,8 @@ 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_SIDESLIP is not set
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_LAND_DETECTOR=y
boards/bitcraze/crazyflie21/default.px4board
+2
View File
@@ -14,6 +14,8 @@ 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_SIDESLIP is not set
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_LAND_DETECTOR=y
boards/px4/fmu-v2/default.px4board
+1
View File
@@ -26,6 +26,7 @@ CONFIG_MODULES_EKF2=y
# CONFIG_EKF2_BARO_COMPENSATION is not set
# CONFIG_EKF2_DRAG_FUSION is not set
# CONFIG_EKF2_GNSS_YAW is not set
# CONFIG_EKF2_SIDESLIP is not set
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_LAND_DETECTOR=y
CONFIG_MODULES_LOGGER=y
src/modules/ekf2/CMakeLists.txt
+8 -2
View File
@@ -68,7 +68,6 @@ add_subdirectory(Utility)
set(EKF_SRCS)
list(APPEND EKF_SRCS
EKF/airspeed_fusion.cpp
EKF/auxvel_fusion.cpp
EKF/baro_height_control.cpp
EKF/bias_estimator.cpp
@@ -99,13 +98,16 @@ list(APPEND EKF_SRCS
EKF/range_finder_consistency_check.cpp
EKF/range_height_control.cpp
EKF/sensor_range_finder.cpp
EKF/sideslip_fusion.cpp
EKF/terrain_estimator.cpp
EKF/vel_pos_fusion.cpp
EKF/zero_innovation_heading_update.cpp
EKF/zero_velocity_update.cpp
)
if(CONFIG_EKF2_AIRSPEED)
list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp)
endif()
if(CONFIG_EKF2_DRAG_FUSION)
list(APPEND EKF_SRCS EKF/drag_fusion.cpp)
endif()
@@ -114,6 +116,10 @@ if(CONFIG_EKF2_GNSS_YAW)
list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
endif()
if(CONFIG_EKF2_SIDESLIP)
list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp)
endif()
px4_add_module(
MODULE modules__ekf2
MAIN ekf2
src/modules/ekf2/EKF/CMakeLists.txt
+8 -2
View File
@@ -33,7 +33,6 @@
set(EKF_SRCS)
list(APPEND EKF_SRCS
airspeed_fusion.cpp
auxvel_fusion.cpp
baro_height_control.cpp
bias_estimator.cpp
@@ -64,13 +63,16 @@ list(APPEND EKF_SRCS
range_finder_consistency_check.cpp
range_height_control.cpp
sensor_range_finder.cpp
sideslip_fusion.cpp
terrain_estimator.cpp
vel_pos_fusion.cpp
zero_innovation_heading_update.cpp
zero_velocity_update.cpp
)
if(CONFIG_EKF2_AIRSPEED)
list(APPEND EKF_SRCS airspeed_fusion.cpp)
endif()
if(CONFIG_EKF2_DRAG_FUSION)
list(APPEND EKF_SRCS drag_fusion.cpp)
endif()
@@ -79,6 +81,10 @@ if(CONFIG_EKF2_GNSS_YAW)
list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
endif()
if(CONFIG_EKF2_SIDESLIP)
list(APPEND EKF_SRCS sideslip_fusion.cpp)
endif()
add_library(ecl_EKF
${EKF_SRCS}
)
src/modules/ekf2/EKF/airspeed_fusion.cpp
-26
View File
@@ -222,21 +222,6 @@ void Ekf::stopAirspeedFusion()
}
}
float Ekf::getTrueAirspeed() const
{
return (_state.vel - Vector3f(_state.wind_vel(0), _state.wind_vel(1), 0.f)).norm();
}
void Ekf::resetWind()
{
if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) {
resetWindUsingAirspeed(_airspeed_sample_delayed);
} else {
resetWindToZero();
}
}
void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample)
{
const float euler_yaw = getEulerYaw(_R_to_earth);
@@ -282,14 +267,3 @@ void Ekf::resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample
P(22, 22) += P(4, 4);
P(23, 23) += P(5, 5);
}
void Ekf::resetWindToZero()
{
ECL_INFO("reset wind to zero");
// If we don't have an airspeed measurement, then assume the wind is zero
_state.wind_vel.setZero();
// start with a small initial uncertainty to improve the initial estimate
P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
}
src/modules/ekf2/EKF/common.h
+5 -1
View File
@@ -307,7 +307,6 @@ struct parameters {
float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec)
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)
float airspeed_delay_ms{100.0f}; ///< airspeed measurement delay relative to the IMU (mSec)
float flow_delay_ms{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec)
float ev_delay_ms{175.0f}; ///< off-board vision measurement delay relative to the IMU (mSec)
@@ -364,16 +363,21 @@ struct parameters {
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)
float tas_innov_gate{5.0f}; ///< True Airspeed innovation consistency gate size (STD)
float eas_noise{1.4f}; ///< EAS measurement noise standard deviation used for airspeed fusion (m/s)
float arsp_thr{2.0f}; ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
// synthetic sideslip fusion
int32_t beta_fusion_enabled{0};
float beta_innov_gate{5.0f}; ///< synthetic sideslip innovation consistency gate size in standard deviation (STD)
float beta_noise{0.3f}; ///< synthetic sideslip noise (rad)
const float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec)
#endif // CONFIG_EKF2_SIDESLIP
// range finder fusion
float range_noise{0.1f}; ///< observation noise for range finder measurements (m)
src/modules/ekf2/EKF/control.cpp
+6
View File
@@ -106,8 +106,14 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
controlMagFusion();
controlOpticalFlowFusion(imu_delayed);
controlGpsFusion(imu_delayed);
#if defined(CONFIG_EKF2_AIRSPEED)
controlAirDataFusion(imu_delayed);
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
controlBetaFusion(imu_delayed);
#endif // CONFIG_EKF2_SIDESLIP
#if defined(CONFIG_EKF2_DRAG_FUSION)
controlDragFusion();
src/modules/ekf2/EKF/ekf.cpp
+4
View File
@@ -116,8 +116,12 @@ void Ekf::reset()
resetEstimatorAidStatus(_aid_src_baro_hgt);
resetEstimatorAidStatus(_aid_src_rng_hgt);
#if defined(CONFIG_EKF2_AIRSPEED)
resetEstimatorAidStatus(_aid_src_airspeed);
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
resetEstimatorAidStatus(_aid_src_sideslip);
#endif // CONFIG_EKF2_SIDESLIP
resetEstimatorAidStatus(_aid_src_fake_pos);
resetEstimatorAidStatus(_aid_src_fake_hgt);
src/modules/ekf2/EKF/ekf.h
+31 -18
View File
@@ -204,13 +204,17 @@ public:
void getDragInnovRatio(float drag_innov_ratio[2]) const { _drag_test_ratio.copyTo(drag_innov_ratio); }
#endif // CONFIG_EKF2_DRAG_FUSION
#if defined(CONFIG_EKF2_AIRSPEED)
void getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _aid_src_airspeed.innovation; }
void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _aid_src_airspeed.innovation_variance; }
void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _aid_src_airspeed.test_ratio; }
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
void getBetaInnov(float &beta_innov) const { beta_innov = _aid_src_sideslip.innovation; }
void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _aid_src_sideslip.innovation_variance; }
void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; }
#endif // CONFIG_EKF2_SIDESLIP
void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; }
void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; }
@@ -233,9 +237,6 @@ public:
// get the wind velocity var
Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); }
// get the true airspeed in m/s
float getTrueAirspeed() const;
// get the full covariance matrix
const matrix::SquareMatrix<float, 24> &covariances() const { return P; }
@@ -444,8 +445,13 @@ public:
const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); }
#if defined(CONFIG_EKF2_AIRSPEED)
const auto &aid_src_airspeed() const { return _aid_src_airspeed; }
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
const auto &aid_src_sideslip() const { return _aid_src_sideslip; }
#endif // CONFIG_EKF2_SIDESLIP
const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
@@ -589,8 +595,12 @@ private:
estimator_aid_source1d_s _aid_src_baro_hgt{};
estimator_aid_source1d_s _aid_src_rng_hgt{};
#if defined(CONFIG_EKF2_AIRSPEED)
estimator_aid_source1d_s _aid_src_airspeed{};
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
estimator_aid_source1d_s _aid_src_sideslip{};
#endif // CONFIG_EKF2_SIDESLIP
estimator_aid_source2d_s _aid_src_fake_pos{};
estimator_aid_source1d_s _aid_src_fake_hgt{};
@@ -725,12 +735,30 @@ private:
// apply sensible limits to the declination and length of the NE mag field states estimates
void limitDeclination();
#if defined(CONFIG_EKF2_AIRSPEED)
// control fusion of air data observations
void controlAirDataFusion(const imuSample &imu_delayed);
void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) const;
void fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src);
void stopAirspeedFusion();
// Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
void resetWindUsingAirspeed(const airspeedSample &airspeed_sample);
// perform a limited reset of the wind state covariances
void resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample);
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
// control fusion of synthetic sideslip observations
void controlBetaFusion(const imuSample &imu_delayed);
// fuse synthetic zero sideslip measurement
void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const;
void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip);
#endif // CONFIG_EKF2_SIDESLIP
#if defined(CONFIG_EKF2_DRAG_FUSION)
// control fusion of multi-rotor drag specific force observations
@@ -956,12 +984,6 @@ private:
void runMagAndMagDeclFusions(const Vector3f &mag);
void run3DMagAndDeclFusions(const Vector3f &mag);
// control fusion of air data observations
void controlAirDataFusion(const imuSample &imu_delayed);
// control fusion of synthetic sideslip observations
void controlBetaFusion(const imuSample &imu_delayed);
// control fusion of fake position observations to constrain drift
void controlFakePosFusion();
@@ -1021,13 +1043,6 @@ private:
// perform a reset of the wind states and related covariances
void resetWind();
// Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
void resetWindUsingAirspeed(const airspeedSample &airspeed_sample);
// perform a limited reset of the wind state covariances
void resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample);
void resetWindToZero();
// check that the range finder data is continuous
@@ -1067,8 +1082,6 @@ private:
return (sensor_timestamp != 0) && (sensor_timestamp + acceptance_interval > _time_latest_us);
}
void stopAirspeedFusion();
void stopGpsFusion();
void stopGpsPosFusion();
void stopGpsVelFusion();
src/modules/ekf2/EKF/ekf_helper.cpp
+35 -1
View File
@@ -693,14 +693,18 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
hgt = NAN;
}
#if defined(CONFIG_EKF2_AIRSPEED)
// return the airspeed fusion innovation test ratio
tas = sqrtf(_aid_src_airspeed.test_ratio);
#endif // CONFIG_EKF2_AIRSPEED
// return the terrain height innovation test ratio
hagl = sqrtf(_hagl_test_ratio);
#if defined(CONFIG_EKF2_SIDESLIP)
// return the synthetic sideslip innovation test ratio
beta = sqrtf(_aid_src_sideslip.test_ratio);
#endif // CONFIG_EKF2_SIDESLIP
}
// return a bitmask integer that describes which state estimates are valid
@@ -775,11 +779,18 @@ void Ekf::updateHorizontalDeadReckoningstatus()
const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
const bool airDataAiding = _control_status.flags.wind &&
bool airDataAiding = false;
#if defined(CONFIG_EKF2_AIRSPEED)
airDataAiding = _control_status.flags.wind &&
isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) &&
isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max);
_control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
#else
_control_status.flags.wind_dead_reckoning = false;
#endif // CONFIG_EKF2_AIRSPEED
_control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
if (!_control_status.flags.inertial_dead_reckoning) {
@@ -1156,3 +1167,26 @@ void Ekf::resetGpsDriftCheckFilters()
_gps_vertical_position_drift_rate_m_s = NAN;
_gps_filtered_horizontal_velocity_m_s = NAN;
}
void Ekf::resetWind()
{
#if defined(CONFIG_EKF2_AIRSPEED)
if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) {
resetWindUsingAirspeed(_airspeed_sample_delayed);
return;
}
#endif // CONFIG_EKF2_AIRSPEED
resetWindToZero();
}
void Ekf::resetWindToZero()
{
ECL_INFO("reset wind to zero");
// If we don't have an airspeed measurement, then assume the wind is zero
_state.wind_vel.setZero();
// start with a small initial uncertainty to improve the initial estimate
P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
}
src/modules/ekf2/EKF/estimator_interface.cpp
+8
View File
@@ -50,7 +50,9 @@ EstimatorInterface::~EstimatorInterface()
delete _mag_buffer;
delete _baro_buffer;
delete _range_buffer;
#if defined(CONFIG_EKF2_AIRSPEED)
delete _airspeed_buffer;
#endif // CONFIG_EKF2_AIRSPEED
delete _flow_buffer;
delete _ext_vision_buffer;
#if defined(CONFIG_EKF2_DRAG_FUSION)
@@ -243,6 +245,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
}
}
#if defined(CONFIG_EKF2_AIRSPEED)
void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
{
if (!_initialised) {
@@ -277,6 +280,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us);
}
}
#endif // CONFIG_EKF2_AIRSPEED
void EstimatorInterface::setRangeData(const rangeSample &range_sample)
{
@@ -523,10 +527,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms);
}
#if defined(CONFIG_EKF2_AIRSPEED)
// using airspeed
if (_params.arsp_thr > FLT_EPSILON) {
max_time_delay_ms = math::max(_params.airspeed_delay_ms, max_time_delay_ms);
}
#endif // CONFIG_EKF2_AIRSPEED
// mag mode
if (_params.mag_fusion_type != MagFuseType::NONE) {
@@ -678,9 +684,11 @@ void EstimatorInterface::print_status()
printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size());
}
#if defined(CONFIG_EKF2_AIRSPEED)
if (_airspeed_buffer) {
printf("airspeed buffer: %d/%d (%d Bytes)\n", _airspeed_buffer->entries(), _airspeed_buffer->get_length(), _airspeed_buffer->get_total_size());
}
#endif // CONFIG_EKF2_AIRSPEED
if (_flow_buffer) {
printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size());
src/modules/ekf2/EKF/estimator_interface.h
+6
View File
@@ -90,7 +90,9 @@ public:
void setBaroData(const baroSample &baro_sample);
#if defined(CONFIG_EKF2_AIRSPEED)
void setAirspeedData(const airspeedSample &airspeed_sample);
#endif // CONFIG_EKF2_AIRSPEED
void setRangeData(const rangeSample &range_sample);
@@ -291,7 +293,9 @@ protected:
// measurement samples capturing measurements on the delayed time horizon
gpsSample _gps_sample_delayed{};
sensor::SensorRangeFinder _range_sensor{};
#if defined(CONFIG_EKF2_AIRSPEED)
airspeedSample _airspeed_sample_delayed{};
#endif // CONFIG_EKF2_AIRSPEED
flowSample _flow_sample_delayed{};
extVisionSample _ev_sample_prev{};
RangeFinderConsistencyCheck _rng_consistency_check;
@@ -348,7 +352,9 @@ protected:
RingBuffer<magSample> *_mag_buffer{nullptr};
RingBuffer<baroSample> *_baro_buffer{nullptr};
RingBuffer<rangeSample> *_range_buffer{nullptr};
#if defined(CONFIG_EKF2_AIRSPEED)
RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
#endif // CONFIG_EKF2_AIRSPEED
RingBuffer<flowSample> *_flow_buffer{nullptr};
RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};
src/modules/ekf2/EKF2.cpp
+41 -7
View File
@@ -65,7 +65,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_gps_delay(_params->gps_delay_ms),
_param_ekf2_of_delay(_params->flow_delay_ms),
_param_ekf2_rng_delay(_params->range_delay_ms),
_param_ekf2_asp_delay(_params->airspeed_delay_ms),
_param_ekf2_ev_delay(_params->ev_delay_ms),
_param_ekf2_avel_delay(_params->auxvel_delay_ms),
_param_ekf2_gyr_noise(_params->gyro_noise),
@@ -86,12 +85,19 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_gnd_max_hgt(_params->gnd_effect_max_hgt),
_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),
_param_ekf2_head_noise(_params->mag_heading_noise),
_param_ekf2_mag_noise(_params->mag_noise),
_param_ekf2_eas_noise(_params->eas_noise),
_param_ekf2_arsp_thr(_params->arsp_thr),
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
_param_ekf2_beta_gate(_params->beta_innov_gate),
_param_ekf2_beta_noise(_params->beta_noise),
_param_ekf2_fuse_beta(_params->beta_fusion_enabled),
#endif // CONFIG_EKF2_SIDESLIP
_param_ekf2_head_noise(_params->mag_heading_noise),
_param_ekf2_mag_noise(_params->mag_noise),
_param_ekf2_mag_decl(_params->mag_declination_deg),
_param_ekf2_hdg_gate(_params->heading_innov_gate),
_param_ekf2_mag_gate(_params->mag_innov_gate),
@@ -151,8 +157,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_ev_pos_x(_params->ev_pos_body(0)),
_param_ekf2_ev_pos_y(_params->ev_pos_body(1)),
_param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
_param_ekf2_arsp_thr(_params->arsp_thr),
_param_ekf2_fuse_beta(_params->beta_fusion_enabled),
_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),
@@ -199,7 +203,9 @@ EKF2::~EKF2()
perf_free(_ecl_ekf_update_full_perf);
perf_free(_msg_missed_imu_perf);
perf_free(_msg_missed_air_data_perf);
#if defined(CONFIG_EKF2_AIRSPEED)
perf_free(_msg_missed_airspeed_perf);
#endif // CONFIG_EKF2_AIRSPEED
perf_free(_msg_missed_distance_sensor_perf);
perf_free(_msg_missed_gps_perf);
perf_free(_msg_missed_landing_target_pose_perf);
@@ -316,7 +322,9 @@ int EKF2::print_status()
perf_print_counter(_ecl_ekf_update_full_perf);
perf_print_counter(_msg_missed_imu_perf);
perf_print_counter(_msg_missed_air_data_perf);
#if defined(CONFIG_EKF2_AIRSPEED)
perf_print_counter(_msg_missed_airspeed_perf);
#endif // CONFIG_EKF2_AIRSPEED
perf_print_counter(_msg_missed_distance_sensor_perf);
perf_print_counter(_msg_missed_gps_perf);
perf_print_counter(_msg_missed_landing_target_pose_perf);
@@ -360,6 +368,7 @@ void EKF2::Run()
_ekf.output_predictor().set_pos_correction_tc(_param_ekf2_tau_pos.get());
_ekf.output_predictor().set_vel_correction_tc(_param_ekf2_tau_vel.get());
#if defined(CONFIG_EKF2_AIRSPEED)
// The airspeed scale factor correcton is only available via parameter as used by the airspeed module
param_t param_aspd_scale = param_find("ASPD_SCALE_1");
@@ -367,6 +376,8 @@ void EKF2::Run()
param_get(param_aspd_scale, &_airspeed_scale_factor);
}
#endif // CONFIG_EKF2_AIRSPEED
// if using baro ensure sensor interval minimum is sufficient to accommodate system averaged baro output
if (_params->baro_ctrl == 1) {
float sens_baro_rate = 0.f;
@@ -612,7 +623,9 @@ void EKF2::Run()
.visual_odometry_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
};
#if defined(CONFIG_EKF2_AIRSPEED)
UpdateAirspeedSample(ekf2_timestamps);
#endif // CONFIG_EKF2_AIRSPEED
UpdateAuxVelSample(ekf2_timestamps);
UpdateBaroSample(ekf2_timestamps);
UpdateExtVisionSample(ekf2_timestamps);
@@ -801,12 +814,14 @@ void EKF2::VerifyParams()
void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
{
#if defined(CONFIG_EKF2_AIRSPEED)
// airspeed
PublishAidSourceStatus(_ekf.aid_src_airspeed(), _status_airspeed_pub_last, _estimator_aid_src_airspeed_pub);
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
// sideslip
PublishAidSourceStatus(_ekf.aid_src_sideslip(), _status_sideslip_pub_last, _estimator_aid_src_sideslip_pub);
#endif // CONFIG_EKF2_SIDESLIP
// baro height
PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub);
@@ -1131,8 +1146,12 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
#if defined(CONFIG_EKF2_DRAG_FUSION)
_ekf.getDragInnov(innovations.drag);
#endif // CONFIG_EKF2_DRAG_FUSION
#if defined(CONFIG_EKF2_AIRSPEED)
_ekf.getAirspeedInnov(innovations.airspeed);
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
_ekf.getBetaInnov(innovations.beta);
#endif // CONFIG_EKF2_SIDESLIP
_ekf.getHaglInnov(innovations.hagl);
_ekf.getHaglRateInnov(innovations.hagl_rate);
_ekf.getTerrainFlowInnov(innovations.terr_flow);
@@ -1184,8 +1203,12 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
#if defined(CONFIG_EKF2_DRAG_FUSION)
_ekf.getDragInnovRatio(&test_ratios.drag[0]);
#endif // CONFIG_EKF2_DRAG_FUSION
#if defined(CONFIG_EKF2_AIRSPEED)
_ekf.getAirspeedInnovRatio(test_ratios.airspeed);
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
_ekf.getBetaInnovRatio(test_ratios.beta);
#endif // CONFIG_EKF2_SIDESLIP
_ekf.getHaglInnovRatio(test_ratios.hagl);
_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
@@ -1213,8 +1236,12 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
#if defined(CONFIG_EKF2_DRAG_FUSION)
_ekf.getDragInnovVar(variances.drag);
#endif // CONFIG_EKF2_DRAG_FUSION
#if defined(CONFIG_EKF2_AIRSPEED)
_ekf.getAirspeedInnovVar(variances.airspeed);
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
_ekf.getBetaInnovVar(variances.beta);
#endif // CONFIG_EKF2_SIDESLIP
_ekf.getHaglInnovVar(variances.hagl);
_ekf.getHaglRateInnovVar(variances.hagl_rate);
_ekf.getTerrainFlowInnovVar(variances.terr_flow);
@@ -1622,10 +1649,15 @@ void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
const Vector2f wind_vel = _ekf.getWindVelocity();
const Vector2f wind_vel_var = _ekf.getWindVelocityVariance();
#if defined(CONFIG_EKF2_AIRSPEED)
_ekf.getAirspeedInnov(wind.tas_innov);
_ekf.getAirspeedInnovVar(wind.tas_innov_var);
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
_ekf.getBetaInnov(wind.beta_innov);
_ekf.getBetaInnovVar(wind.beta_innov_var);
#endif // CONFIG_EKF2_SIDESLIP
wind.windspeed_north = wind_vel(0);
wind.windspeed_east = wind_vel(1);
@@ -1682,6 +1714,7 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
return amsl_hgt + _wgs84_hgt_offset;
}
#if defined(CONFIG_EKF2_AIRSPEED)
void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF airspeed sample
@@ -1748,6 +1781,7 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
}
}
}
#endif // CONFIG_EKF2_AIRSPEED
void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
{
src/modules/ekf2/EKF2.hpp
+46 -26
View File
@@ -64,8 +64,6 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/estimator_bias.h>
@@ -97,6 +95,11 @@
#include <uORB/topics/wind.h>
#include <uORB/topics/yaw_estimator_status.h>
#if defined(CONFIG_EKF2_AIRSPEED)
# include <uORB/topics/airspeed.h>
# include <uORB/topics/airspeed_validated.h>
#endif // CONFIG_EKF2_AIRSPEED
extern pthread_mutex_t ekf2_module_mutex;
class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
@@ -165,7 +168,9 @@ private:
void PublishWindEstimate(const hrt_abstime &timestamp);
void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
#if defined(CONFIG_EKF2_AIRSPEED)
void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
#endif // CONFIG_EKF2_AIRSPEED
void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps);
@@ -227,8 +232,6 @@ private:
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_air_data_perf{nullptr};
perf_counter_t _msg_missed_airspeed_perf{nullptr};
perf_counter_t _msg_missed_airspeed_validated_perf{nullptr};
perf_counter_t _msg_missed_distance_sensor_perf{nullptr};
perf_counter_t _msg_missed_gps_perf{nullptr};
perf_counter_t _msg_missed_landing_target_pose_perf{nullptr};
@@ -265,9 +268,6 @@ private:
hrt_abstime _last_sensor_bias_published{0};
hrt_abstime _last_gps_status_published{0};
hrt_abstime _status_airspeed_pub_last{0};
hrt_abstime _status_sideslip_pub_last{0};
hrt_abstime _status_baro_hgt_pub_last{0};
hrt_abstime _status_rng_hgt_pub_last{0};
@@ -301,16 +301,30 @@ private:
float _last_rng_hgt_bias_published{};
matrix::Vector3f _last_ev_bias_published{};
#if defined(CONFIG_EKF2_AIRSPEED)
uORB::Subscription _airspeed_sub {ORB_ID(airspeed)};
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
hrt_abstime _airspeed_validated_timestamp_last{0};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_airspeed_pub {ORB_ID(estimator_aid_src_airspeed)};
hrt_abstime _status_airspeed_pub_last{0};
perf_counter_t _msg_missed_airspeed_perf{nullptr};
perf_counter_t _msg_missed_airspeed_validated_perf{nullptr};
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_sideslip_pub {ORB_ID(estimator_aid_src_sideslip)};
hrt_abstime _status_sideslip_pub_last {0};
#endif // CONFIG_EKF2_SIDESLIP
orb_advert_t _mavlink_log_pub{nullptr};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
@@ -361,10 +375,8 @@ private:
uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_airspeed_pub{ORB_ID(estimator_aid_src_airspeed)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_sideslip_pub{ORB_ID(estimator_aid_src_sideslip)};
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)};
@@ -419,8 +431,6 @@ private:
_param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
(ParamExtFloat<px4::params::EKF2_RNG_DELAY>)
_param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_ASP_DELAY>)
_param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_EV_DELAY>)
_param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec)
(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
@@ -464,19 +474,35 @@ private:
_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)
(ParamExtFloat<px4::params::EKF2_TAS_GATE>)
_param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD)
_param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_EAS_NOISE>)
_param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec)
// control of airspeed fusion
(ParamExtFloat<px4::params::EKF2_ARSP_THR>)
_param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
(ParamExtFloat<px4::params::EKF2_BETA_GATE>)
_param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_BETA_NOISE>) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad)
(ParamExtInt<px4::params::EKF2_FUSE_BETA>)
_param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables
#endif // CONFIG_EKF2_SIDESLIP
// control of magnetometer fusion
(ParamExtFloat<px4::params::EKF2_HEAD_NOISE>)
_param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad)
(ParamExtFloat<px4::params::EKF2_MAG_NOISE>)
_param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss)
(ParamExtFloat<px4::params::EKF2_EAS_NOISE>)
_param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec)
(ParamExtFloat<px4::params::EKF2_BETA_GATE>)
_param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_BETA_NOISE>) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad)
(ParamExtFloat<px4::params::EKF2_MAG_DECL>) _param_ekf2_mag_decl,///< magnetic declination (degrees)
(ParamExtFloat<px4::params::EKF2_HDG_GATE>)
_param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD)
@@ -586,12 +612,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)
// control of airspeed and sideslip fusion
(ParamExtFloat<px4::params::EKF2_ARSP_THR>)
_param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
(ParamExtInt<px4::params::EKF2_FUSE_BETA>)
_param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables
// 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)
src/modules/ekf2/Kconfig
+15
View File
@@ -13,6 +13,14 @@ depends on MODULES_EKF2
---help---
EKF2 support multiple instances and selector.
menuconfig EKF2_AIRSPEED
depends on MODULES_EKF2
bool "airspeed fusion support"
default y
depends on EKF2_SIDESLIP
---help---
EKF2 airspeed fusion support.
menuconfig EKF2_BARO_COMPENSATION
depends on MODULES_EKF2
bool "barometer compensation support"
@@ -34,6 +42,13 @@ depends on MODULES_EKF2
---help---
EKF2 GNSS yaw fusion support.
menuconfig EKF2_SIDESLIP
depends on MODULES_EKF2
bool "sideslip fusion support"
default y
---help---
EKF2 sideslip fusion support.
menuconfig USER_EKF2
bool "ekf2 running as userspace module"
default n