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

ekf2: new gravity observation (#21038)

Browse Source 
Signed-off-by: Daniel M. Sahu <danielmohansahu@gmail.com>
This commit is contained in:
Daniel Sahu
2023-02-07 13:28:58 -05:00
committed by GitHub
parent 3e149ee6c5
commit fa6fda6cce
15 changed files with 456 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+11
View File
@@ -191,6 +191,10 @@ public:
void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
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(); }
// get the state vector at the delayed time horizon
matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const;
@@ -433,6 +437,8 @@ public:
const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
const auto &aid_src_mag() const { return _aid_src_mag; }
const auto &aid_src_gravity() const { return _aid_src_gravity; }
const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
@@ -573,6 +579,8 @@ private:
estimator_aid_source1d_s _aid_src_mag_heading{};
estimator_aid_source3d_s _aid_src_mag{};
estimator_aid_source3d_s _aid_src_gravity{};
estimator_aid_source2d_s _aid_src_aux_vel{};
estimator_aid_source2d_s _aid_src_optical_flow{};
@@ -927,6 +935,9 @@ private:
void updateGroundEffect();
// gravity fusion: heuristically enable / disable gravity fusion
void controlGravityFusion(const imuSample &imu_delayed);
// calculate the measurement variance for the optical flow sensor
float calcOptFlowMeasVar(const flowSample &flow_sample);