dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Save mag field covariance data before reset

This commit is contained in:
Paul Riseborough 2019-01-17 17:36:11 +11:00 committed by Daniel Agar
parent 82ce7a83a5
commit d52f53635b
3 changed files with 77 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
EKF/control.cpp
View File

@ -239,12 +239,17 @@ void Ekf::controlExternalVisionFusion()
// flag the yaw as aligned
_control_status.flags.yaw_align = true;
// turn on fusion of external vision yaw measurements and disable all magnetoemter fusion
// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
_control_status.flags.ev_yaw = true;
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false;
_control_status.flags.mag_dec = false;
// save covariance data for re-use if currently doing 3-axis fusion
if (_control_status.flags.mag_3D) {
save_mag_cov_data();
_control_status.flags.mag_3D = false;
}
ECL_INFO("EKF commencing external vision yaw fusion");
}
}
@ -530,9 +535,14 @@ void Ekf::controlGpsFusion()
_control_status.flags.gps_yaw = true;
_control_status.flags.ev_yaw = false;
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false;
_control_status.flags.mag_dec = false;
// save covariance data for re-use if currently doing 3-axis fusion
if (_control_status.flags.mag_3D) {
save_mag_cov_data();
_control_status.flags.mag_3D = false;
}
ECL_INFO("EKF commencing GPS yaw fusion");
}
}
@ -1346,11 +1356,17 @@ void Ekf::controlDragFusion()
void Ekf::controlMagFusion()
{
if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) {
// do not use the magnetomer and deactivate magnetic field states
// save covariance data for re-use if currently doing 3-axis fusion
if (_control_status.flags.mag_3D) {
save_mag_cov_data();
_control_status.flags.mag_3D = false;
}
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false;
return;
}
@ -1467,7 +1483,6 @@ void Ekf::controlMagFusion()
for (uint8_t index = 0; index <= 3; index ++) {
P[index + 18][index + 18] = _saved_mag_bf_variance[index];
}
// re-instate the NE axis covariance sub-matrix
for (uint8_t row = 0; row <= 1; row ++) {
for (uint8_t col = 0; col <= 1; col ++) {
@ -1482,7 +1497,7 @@ void Ekf::controlMagFusion()
_control_status.flags.mag_hdg = !_control_status.flags.mag_3D;
} else {
// save magnetic field state covariance data for next time
// save covariance data for re-use if currently doing 3-axis fusion
if (_control_status.flags.mag_3D) {
save_mag_cov_data();
_control_status.flags.mag_3D = false;
@ -1515,17 +1530,24 @@ void Ekf::controlMagFusion()
P[index + 16][index + 16] = sq(_params.mag_noise);
}
// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
fuseDeclination(0.02f);
if (_control_status.flags.mag_3D) {
// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
fuseDeclination(0.02f);
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
save_mag_cov_data();
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
save_mag_cov_data();
}
}
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
// always use heading fusion
_control_status.flags.mag_hdg = true;
_control_status.flags.mag_3D = false;
// save covariance data for re-use if currently doing 3-axis fusion
if (_control_status.flags.mag_3D) {
save_mag_cov_data();
_control_status.flags.mag_3D = false;
}
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
@ -1541,7 +1563,13 @@ void Ekf::controlMagFusion()
} else {
// do no magnetometer fusion at all
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false;
// save covariance data for re-use if currently doing 3-axis fusion
if (_control_status.flags.mag_3D) {
save_mag_cov_data();
_control_status.flags.mag_3D = false;
}
}
// if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift

61
EKF/ekf_helper.cpp
View File

@ -487,16 +487,18 @@ bool Ekf::realignYawGPS()
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
for (uint8_t index = 16; index <= 21; index ++) {
P[index][index] = sq(_params.mag_noise);
if (_control_status.flags.mag_3D) {
for (uint8_t index = 16; index <= 21; index ++) {
P[index][index] = sq(_params.mag_noise);
}
// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
fuseDeclination(0.02f);
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
save_mag_cov_data();
}
// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
fuseDeclination(0.02f);
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
save_mag_cov_data();
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
@ -528,16 +530,18 @@ bool Ekf::realignYawGPS()
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
for (uint8_t index = 16; index <= 21; index ++) {
P[index][index] = sq(_params.mag_noise);
if (_control_status.flags.mag_3D) {
for (uint8_t index = 16; index <= 21; index ++) {
P[index][index] = sq(_params.mag_noise);
}
// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
fuseDeclination(0.02f);
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
save_mag_cov_data();
}
// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
fuseDeclination(0.02f);
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
save_mag_cov_data();
// record the start time for the magnetic field alignment
_flt_mag_align_start_time = _imu_sample_delayed.time_us;
@ -561,10 +565,15 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) {
// do not use the magnetomer and deactivate magnetic field states
// save covariance data for re-use if currently doing 3-axis fusion
if (_control_status.flags.mag_3D) {
save_mag_cov_data();
_control_status.flags.mag_3D = false;
}
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
_control_status.flags.mag_hdg = false;
_control_status.flags.mag_3D = false;
return false;
}
@ -693,16 +702,18 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
for (uint8_t index = 16; index <= 21; index ++) {
P[index][index] = sq(_params.mag_noise);
if (_control_status.flags.mag_3D) {
for (uint8_t index = 16; index <= 21; index ++) {
P[index][index] = sq(_params.mag_noise);
}
// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
fuseDeclination(0.02f);
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
save_mag_cov_data();
}
// Fuse the declination angle to prevent rapid rotation of earth field vector estimates
fuseDeclination(0.02f);
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
save_mag_cov_data();
// record the time for the magnetic field alignment event
_flt_mag_align_start_time = _imu_sample_delayed.time_us;

2
EKF/mag_fusion.cpp
View File

@ -916,7 +916,7 @@ void Ekf::fuseDeclination(float decl_sigma)
}
}
// correct the covariance marix for gross errors
// correct the covariance matrix for gross errors
fixCovarianceErrors();
// apply the state corrections