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

ekf2: add kconfig option to enable/disable AUX velocity fusion

Browse Source
This commit is contained in:
Daniel Agar
2023-03-20 10:12:17 -04:00
parent 4270a303ab
commit d47f96f1a5
13 changed files with 84 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/CMakeLists.txt
+4 -1
View File
@@ -33,7 +33,6 @@
set(EKF_SRCS)
list(APPEND EKF_SRCS
auxvel_fusion.cpp
baro_height_control.cpp
bias_estimator.cpp
control.cpp
@@ -73,6 +72,10 @@ if(CONFIG_EKF2_AIRSPEED)
list(APPEND EKF_SRCS airspeed_fusion.cpp)
endif()
if(CONFIG_EKF2_AUXVEL)
list(APPEND EKF_SRCS auxvel_fusion.cpp)
endif()
if(CONFIG_EKF2_DRAG_FUSION)
list(APPEND EKF_SRCS drag_fusion.cpp)
endif()
src/modules/ekf2/EKF/common.h
+5 -1
View File
@@ -259,11 +259,13 @@ struct dragSample {
};
#endif // CONFIG_EKF2_DRAG_FUSION
#if defined(CONFIG_EKF2_AUXVEL)
struct auxVelSample {
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
Vector2f vel{}; ///< measured NE velocity relative to the local origin (m/sec)
Vector2f velVar{}; ///< estimated error variance of the NE velocity (m/sec)**2
};
#endif // CONFIG_EKF2_AUXVEL
struct systemFlagUpdate {
uint64_t time_us{};
@@ -310,7 +312,6 @@ struct parameters {
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)
float auxvel_delay_ms{5.0f}; ///< auxiliary velocity 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)
@@ -469,9 +470,12 @@ struct parameters {
const float vert_innov_test_min{1.0f}; ///< Minimum number of standard deviations of vertical vel/pos innovations required to trigger a vertical acceleration failure
const int bad_acc_reset_delay_us{500000}; ///< Continuous time that the vertical position and velocity innovation test must fail before the states are reset (uSec)
#if defined(CONFIG_EKF2_AUXVEL)
// auxiliary velocity fusion
float auxvel_delay_ms{5.0f}; ///< auxiliary velocity measurement delay relative to the IMU (mSec)
const float auxvel_noise{0.5f}; ///< minimum observation noise, uses reported noise if greater (m/s)
const float auxvel_gate{5.0f}; ///< velocity fusion innovation consistency gate size (STD)
#endif // CONFIG_EKF2_AUXVEL
// compute synthetic magnetomter Z value if possible
int32_t synthesize_mag_z{0};
src/modules/ekf2/EKF/control.cpp
+2
View File
@@ -125,8 +125,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
// Additional data odometry data from an external estimator can be fused.
controlExternalVisionFusion();
#if defined(CONFIG_EKF2_AUXVEL)
// Additional horizontal velocity data from an auxiliary sensor can be fused
controlAuxVelFusion();
#endif // CONFIG_EKF2_AUXVEL
controlZeroInnovationHeadingUpdate();
src/modules/ekf2/EKF/ekf.cpp
+2
View File
@@ -142,7 +142,9 @@ void Ekf::reset()
resetEstimatorAidStatus(_aid_src_mag_heading);
resetEstimatorAidStatus(_aid_src_mag);
#if defined(CONFIG_EKF2_AUXVEL)
resetEstimatorAidStatus(_aid_src_aux_vel);
#endif // CONFIG_EKF2_AUXVEL
resetEstimatorAidStatus(_aid_src_optical_flow);
resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
src/modules/ekf2/EKF/ekf.h
+9 -2
View File
@@ -98,9 +98,11 @@ public:
void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; }
void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; }
#if defined(CONFIG_EKF2_AUXVEL)
void getAuxVelInnov(float aux_vel_innov[2]) const;
void getAuxVelInnovVar(float aux_vel_innov[2]) const;
void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); }
#endif // CONFIG_EKF2_AUXVEL
void getFlowInnov(float flow_innov[2]) const;
void getFlowInnovVar(float flow_innov_var[2]) const;
@@ -477,7 +479,9 @@ public:
const auto &aid_src_gravity() const { return _aid_src_gravity; }
#if defined(CONFIG_EKF2_AUXVEL)
const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
#endif // CONFIG_EKF2_AUXVEL
const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
@@ -624,7 +628,9 @@ private:
estimator_aid_source3d_s _aid_src_gravity{};
#if defined(CONFIG_EKF2_AUXVEL)
estimator_aid_source2d_s _aid_src_aux_vel{};
#endif // CONFIG_EKF2_AUXVEL
estimator_aid_source2d_s _aid_src_optical_flow{};
estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
@@ -996,8 +1002,11 @@ private:
void controlZeroInnovationHeadingUpdate();
#if defined(CONFIG_EKF2_AUXVEL)
// control fusion of auxiliary velocity observations
void controlAuxVelFusion();
void stopAuxVelFusion();
#endif // CONFIG_EKF2_AUXVEL
void checkVerticalAccelerationHealth(const imuSample &imu_delayed);
Likelihood estimateInertialNavFallingLikelihood() const;
@@ -1089,8 +1098,6 @@ private:
void stopEvVelFusion();
void stopEvYawFusion();
void stopAuxVelFusion();
void stopFlowFusion();
void resetFakePosFusion();
src/modules/ekf2/EKF/ekf_helper.cpp
+2
View File
@@ -335,6 +335,7 @@ void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vp
vpos = _aid_src_ev_hgt.test_ratio;
}
#if defined(CONFIG_EKF2_AUXVEL)
void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const
{
aux_vel_innov[0] = _aid_src_aux_vel.innovation[0];
@@ -346,6 +347,7 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
aux_vel_innov_var[0] = _aid_src_aux_vel.innovation_variance[0];
aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1];
}
#endif // CONFIG_EKF2_AUXVEL
void Ekf::getFlowInnov(float flow_innov[2]) const
{
src/modules/ekf2/EKF/estimator_interface.cpp
+10 -1
View File
@@ -58,7 +58,9 @@ EstimatorInterface::~EstimatorInterface()
#if defined(CONFIG_EKF2_DRAG_FUSION)
delete _drag_buffer;
#endif // CONFIG_EKF2_DRAG_FUSION
#if defined(CONFIG_EKF2_AUXVEL)
delete _auxvel_buffer;
#endif // CONFIG_EKF2_AUXVEL
}
// Accumulate imu data and store to buffer at desired rate
@@ -391,6 +393,7 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
}
}
#if defined(CONFIG_EKF2_AUXVEL)
void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
{
if (!_initialised) {
@@ -425,6 +428,7 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, _min_obs_interval_us);
}
}
#endif // CONFIG_EKF2_AUXVEL
void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
{
@@ -520,7 +524,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
// find the maximum time delay the buffers are required to handle
// it's reasonable to assume that aux velocity device has low delay. TODO: check the delay only if the aux device is used
float max_time_delay_ms = math::max((float)_params.sensor_interval_max_ms, _params.auxvel_delay_ms);
float max_time_delay_ms = _params.sensor_interval_max_ms;
// aux vel
#if defined(CONFIG_EKF2_AUXVEL)
max_time_delay_ms = math::max(_params.auxvel_delay_ms, max_time_delay_ms);
#endif // CONFIG_EKF2_AUXVEL
// using baro
if (_params.baro_ctrl > 0) {
src/modules/ekf2/EKF/estimator_interface.h
+4
View File
@@ -102,7 +102,9 @@ public:
// set external vision position and attitude data
void setExtVisionData(const extVisionSample &evdata);
#if defined(CONFIG_EKF2_AUXVEL)
void setAuxVelData(const auxVelSample &auxvel_sample);
#endif // CONFIG_EKF2_AUXVEL
void setSystemFlagData(const systemFlagUpdate &system_flags);
@@ -357,7 +359,9 @@ protected:
#endif // CONFIG_EKF2_AIRSPEED
RingBuffer<flowSample> *_flow_buffer{nullptr};
RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
#if defined(CONFIG_EKF2_AUXVEL)
RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};
#endif // CONFIG_EKF2_AUXVEL
RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
uint64_t _time_last_gps_buffer_push{0};