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

ekf2: centralize mag covariance reset (#693)

Browse Source 
* ekf2: centralize mag covariance reset. A complete and clean reset of the
mag states covariances is now performed through the "resetMagCov"
function only. This avoid having slight differences of implementations
across the code.

* Ekf: reset quat cov with initial uncertainty instead of zero
This commit is contained in:
Mathieu Bresciani 2020-03-17 10:07:20 +01:00 committed by GitHub
parent f1aa53db8a
commit f20fc08b7d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 39 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
EKF/covariance.cpp
View File

@ -57,11 +57,7 @@ void Ekf::initialiseCovariance()
const float dt = FILTER_UPDATE_PERIOD_S;
// define the initial angle uncertainty as variances for a rotation vector
Vector3f rot_vec_var;
rot_vec_var.setAll(sq(_params.initial_tilt_err));
initialiseQuatCovariances(rot_vec_var);
resetQuatCov();
// velocity
P(4,4) = sq(fmaxf(_params.gps_vel_noise, 0.01f));
@ -97,14 +93,7 @@ void Ekf::initialiseCovariance()
// record IMU bias state covariance reset time - used to prevent resets being performed too often
_last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us;
// earth frame and body frame magnetic field
// set to observation variance
for (uint8_t index = 16; index <= 21; index ++) {
P(index,index) = sq(_params.mag_noise);
}
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
saveMagCovData();
resetMagCov();
// wind
P(22,22) = sq(_params.initial_wind_uncertainty);
@ -869,16 +858,41 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
void Ekf::resetMagRelatedCovariances()
{
// set the quaternion covariance terms to zero
resetQuatCov();
resetMagCov();
}
void Ekf::resetQuatCov(){
zeroQuatCov();
// define the initial angle uncertainty as variances for a rotation vector
Vector3f rot_vec_var;
rot_vec_var.setAll(sq(_params.initial_tilt_err));
initialiseQuatCovariances(rot_vec_var);
}
void Ekf::zeroQuatCov()
{
P.uncorrelateCovarianceSetVariance<2>(0, 0.0f);
P.uncorrelateCovarianceSetVariance<2>(2, 0.0f);
}
void Ekf::resetMagCov()
{
// reset the corresponding rows and columns in the covariance matrix and
// set the variances on the magnetic field states to the measurement variance
clearMagCov();
// reset the field state variance to the observation variance
P.uncorrelateCovarianceSetVariance<3>(16, sq(_params.mag_noise));
P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
saveMagCovData();
if (!_control_status.flags.mag_3D) {
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
// if already in 3-axis fusion mode, the covariances are automatically saved when switching out
// of this mode
saveMagCovData();
}
}
void Ekf::clearMagCov()

6
EKF/ekf.h
View File

@ -760,9 +760,13 @@ private:
// do not call before quaternion states are initialised
void initialiseQuatCovariances(Vector3f &rot_vec_var);
// perform a limited reset of the magnetic field state covariances
// perform a limited reset of the magnetic field related state covariances
void resetMagRelatedCovariances();
void resetQuatCov();
void zeroQuatCov();
void resetMagCov();
// perform a limited reset of the wind state covariances
void resetWindCovariance();

36
EKF/ekf_helper.cpp
View File

@ -459,17 +459,7 @@ bool Ekf::realignYawGPS()
_R_to_earth = Dcmf(_state.quat_nominal);
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
// reset the corresponding rows and columns in the covariance matrix and set the
// variances on the magnetic field states to the measurement variance
clearMagCov();
if (_control_status.flags.mag_3D) {
for (uint8_t index = 16; index <= 21; index ++) {
P(index,index) = sq(_params.mag_noise);
}
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
saveMagCovData();
}
resetMagCov();
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
@ -485,17 +475,7 @@ bool Ekf::realignYawGPS()
// calculate initial earth magnetic field states
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
clearMagCov();
if (_control_status.flags.mag_3D) {
for (uint8_t index = 16; index <= 21; index ++) {
P(index,index) = sq(_params.mag_noise);
}
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
saveMagCovData();
}
resetMagCov();
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
@ -579,17 +559,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
// set the earth magnetic field states using the updated rotation
_state.mag_I = _R_to_earth * mag_init;
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
clearMagCov();
if (_control_status.flags.mag_3D) {
for (uint8_t index = 16; index <= 21; index ++) {
P(index,index) = sq(_params.mag_noise);
}
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
saveMagCovData();
}
resetMagCov();
// record the time for the magnetic field alignment event
_flt_mag_align_start_time = _imu_sample_delayed.time_us;