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

ekf2: cleanup zero innovation heading fusion

Browse Source 
- refactor updateQuaternion() to compute the yaw jacobian directly (respecting  the rotation sequence determination)
 - fuseHeading()/fuseYaw321()/fuseYaw312() helpers are eliminated and now mag heading fusion and EV yaw fusion compute the innovation in place
 - clear up logic for performing zero innovation heading fusion when quaternion variance exceeds threshold (no more _is_yaw_fusion_inhibited flag manipulation)
 - when at rest continue fusing last static heading with very low variance even if other heading sources are active
This commit is contained in:
Daniel Agar
2022-07-19 11:58:27 -04:00
parent ed14151734
commit 39453405a0
11 changed files with 948 additions and 990 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+6 -20
View File
@@ -412,6 +412,8 @@ private:
uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of vertical position measurements was performed (uSec)
uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec)
uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec)
uint64_t _time_last_heading_fuse{0};
uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec)
uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec)
@@ -605,28 +607,10 @@ private:
// ekf sequential fusion of magnetometer measurements
void fuseMag(const Vector3f &mag);
// fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer)
void fuseHeading(float measured_hdg = NAN, float obs_var = NAN);
// fuse the yaw angle defined as the first rotation in a 321 Tait-Bryan rotation sequence
// yaw : angle observation defined as the first rotation in a 321 Tait-Bryan rotation sequence (rad)
// yaw_variance : variance of the yaw angle observation (rad^2)
// zero_innovation : Fuse data with innovation set to zero
bool fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation = false);
// fuse the yaw angle defined as the first rotation in a 312 Tait-Bryan rotation sequence
// yaw : angle observation defined as the first rotation in a 312 Tait-Bryan rotation sequence (rad)
// yaw_variance : variance of the yaw angle observation (rad^2)
// zero_innovation : Fuse data with innovation set to zero
bool fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation = false);
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
// update quaternion states and covariances using a yaw innovation and yaw observation variance
// innovation : prediction - measurement
// variance : observaton variance
// gate_sigma : innovation consistency check gate size (Sigma)
// jacobian : 4x1 vector of partial derivatives of observation wrt each quaternion state
bool updateQuaternion(const float innovation, const float variance, const float gate_sigma,
const Vector4f &yaw_jacobian);
bool fuseYaw(const float innovation, const float variance);
// fuse the yaw angle obtained from a dual antenna GPS unit
void fuseGpsYaw();
@@ -910,6 +894,8 @@ private:
void controlZeroVelocityUpdate();
void controlZeroInnovationHeadingUpdate();
// control fusion of auxiliary velocity observations
void controlAuxVelFusion();