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

ekf2: add kconfig to disable gravity fusion (#22231)

Browse Source
This commit is contained in:
Daniel Agar
2023-10-18 10:50:51 -04:00
committed by GitHub
parent 6eae9fb371
commit 3d238b0275
8 changed files with 67 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/CMakeLists.txt
+4 -1
View File
@@ -42,7 +42,6 @@ list(APPEND EKF_SRCS
estimator_interface.cpp
fake_height_control.cpp
fake_pos_control.cpp
gravity_fusion.cpp
height_control.cpp
imu_down_sampler.cpp
output_predictor.cpp
@@ -93,6 +92,10 @@ if(CONFIG_EKF2_GNSS_YAW)
list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
endif()
if(CONFIG_EKF2_GRAVITY_FUSION)
list(APPEND EKF_SRCS gravity_fusion.cpp)
endif()
if(CONFIG_EKF2_MAGNETOMETER)
list(APPEND EKF_SRCS
mag_3d_control.cpp
src/modules/ekf2/EKF/common.h
+2
View File
@@ -454,8 +454,10 @@ struct parameters {
Vector3f ev_pos_body{}; ///< xyz position of VI-sensor focal point in body frame (m)
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
// gravity fusion
float gravity_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2)
#endif // CONFIG_EKF2_GRAVITY_FUSION
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
int32_t flow_ctrl{0};
src/modules/ekf2/EKF/control.cpp
+3
View File
@@ -128,7 +128,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
#endif // CONFIG_EKF2_DRAG_FUSION
controlHeightFusion(imu_delayed);
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
controlGravityFusion(imu_delayed);
#endif // CONFIG_EKF2_GRAVITY_FUSION
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
// Additional data odometry data from an external estimator can be fused.
src/modules/ekf2/EKF/ekf.h
+8 -2
View File
@@ -310,9 +310,13 @@ public:
void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; }
#endif // CONFIG_EKF2_SIDESLIP
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
const auto &aid_src_gravity() const { return _aid_src_gravity; }
void getGravityInnov(float grav_innov[3]) const { memcpy(grav_innov, _aid_src_gravity.innovation, sizeof(_aid_src_gravity.innovation)); }
void getGravityInnovVar(float grav_innov_var[3]) const { memcpy(grav_innov_var, _aid_src_gravity.innovation_variance, sizeof(_aid_src_gravity.innovation_variance)); }
void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); }
#endif // CONFIG_EKF2_GRAVITY_FUSION
// get the state vector at the delayed time horizon
const matrix::Vector<float, State::size> &getStateAtFusionHorizonAsVector() const { return _state.vector(); }
@@ -554,8 +558,6 @@ public:
const auto &aid_src_mag() const { return _aid_src_mag; }
#endif // CONFIG_EKF2_MAGNETOMETER
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
@@ -731,7 +733,9 @@ private:
# endif // CONFIG_EKF2_GNSS_YAW
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
estimator_aid_source3d_s _aid_src_gravity{};
#endif // CONFIG_EKF2_GRAVITY_FUSION
#if defined(CONFIG_EKF2_AUXVEL)
estimator_aid_source2d_s _aid_src_aux_vel{};
@@ -1184,8 +1188,10 @@ private:
void updateGroundEffect();
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
// gravity fusion: heuristically enable / disable gravity fusion
void controlGravityFusion(const imuSample &imu_delayed);
#endif // CONFIG_EKF2_GRAVITY_FUSION
void resetQuatCov(const float yaw_noise = NAN);
void resetQuatCov(const Vector3f &euler_noise_ned);