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

ekf2: change delta angle and delta velocity bias states to accel and gyro bias (#21901)

Browse Source 
* ekf2-test: remove outdated codegen comparison

The definition of states changed so the comparison with the old
derivation cannot work anymore.

---------

Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
2023-07-28 09:31:44 -04:00
committed by GitHub
co-authored by bresch
parent 444e5d2d4a
commit 84b6b472b4
27 changed files with 1452 additions and 2374 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+8 -10
View File
@@ -363,13 +363,13 @@ public:
}
// gyro bias (states 10, 11, 12)
Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s
Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / sq(_dt_ekf_avg); } // get the gyroscope bias variance in rad/s
const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s
Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)}; } // get the gyroscope bias variance in rad/s
float getGyroBiasLimit() const { return _params.gyro_bias_lim; }
// accel bias (states 13, 14, 15)
Vector3f getAccelBias() const { return _state.delta_vel_bias / _dt_ekf_avg; } // get the accelerometer bias in m/s**2
Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)} / sq(_dt_ekf_avg); } // get the accelerometer bias variance in m/s**2
const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2
Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)}; } // get the accelerometer bias variance in m/s**2
float getAccelBiasLimit() const { return _params.acc_bias_lim; }
const Vector3f &getMagEarthField() const { return _state.mag_I; }
@@ -589,9 +589,6 @@ private:
SquareMatrix24f P{}; ///< state covariance matrix
Vector3f _delta_angle_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta angle bias variance
Vector3f _delta_vel_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta velocity bias variance
#if defined(CONFIG_EKF2_DRAG_FUSION)
Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2)
Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2)
@@ -721,8 +718,9 @@ private:
Vector3f _accel_vec_filt{}; ///< acceleration vector after application of a low pass filter (m/sec**2)
float _accel_magnitude_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
float _ang_rate_magnitude_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec)
Vector3f _prev_delta_ang_bias_var{}; ///< saved delta angle XYZ bias variances (rad/sec)
Vector3f _prev_dvel_bias_var{}; ///< saved delta velocity XYZ bias variances (m/sec)**2
Vector3f _prev_gyro_bias_var{}; ///< saved gyro XYZ bias variances
Vector3f _prev_accel_bias_var{}; ///< saved accel XYZ bias variances
// height sensor status
bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags
@@ -1110,7 +1108,7 @@ private:
void clearMagCov();
void zeroMagCov();
void resetZDeltaAngBiasCov();
void resetGyroBiasZCov();
// uncorrelate quaternion states from other states
void uncorrelateQuatFromOtherStates();