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

ekf2: use bias corrected angular velocity

Browse Source 
- avoid unnecessarily storing _ang_rate_delayed_raw
This commit is contained in:
Daniel Agar
2024-07-10 21:20:47 -04:00
parent 20137bea40
commit f93dc61770
11 changed files with 49 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+16 -14
View File
@@ -566,8 +566,6 @@ private:
StateResets _state_reset_status{}; ///< reset event monitoring structure containing velocity, position, height and yaw reset information
StateResetCounts _state_reset_count_prev{};
Vector3f _ang_rate_delayed_raw{}; ///< uncorrected angular rate vector at fusion time horizon (rad/sec)
StateSample _state{}; ///< state struct of the ekf running at the delayed time horizon
bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised
@@ -936,16 +934,20 @@ private:
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
// control fusion of external vision observations
void controlExternalVisionFusion();
void controlExternalVisionFusion(const imuSample &imu_sample);
void updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset);
void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src);
void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
void controlEvHeightFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
estimator_aid_source1d_s &aid_src);
void controlEvPosFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
estimator_aid_source2d_s &aid_src);
void controlEvVelFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
estimator_aid_source3d_s &aid_src);
void controlEvYawFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
estimator_aid_source1d_s &aid_src);
void resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame);
Vector3f rotateVarianceToEkf(const Vector3f &measurement_var);
@@ -968,7 +970,7 @@ private:
// control fusion of GPS observations
void controlGpsFusion(const imuSample &imu_delayed);
void stopGpsFusion();
void updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src);
void updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src);
void updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src);
void controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel);
bool tryYawEmergencyReset();
@@ -1063,13 +1065,13 @@ private:
void checkHeightSensorRefFallback();
#if defined(CONFIG_EKF2_BAROMETER)
void controlBaroHeightFusion();
void controlBaroHeightFusion(const imuSample &imu_sample);
void stopBaroHgtFusion();
void updateGroundEffect();
# if defined(CONFIG_EKF2_BARO_COMPENSATION)
float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const;
float compensateBaroForDynamicPressure(const imuSample &imu_sample, float baro_alt_uncompensated) const;
# endif // CONFIG_EKF2_BARO_COMPENSATION
#endif // CONFIG_EKF2_BAROMETER