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

Make covariance matrices of type matrix::SquareMatrix

Browse Source 
and use functions from Matrix to reset covariances.
This commit is contained in:
kamilritz
2020-01-02 19:26:57 +11:00
committed by Paul Riseborough
parent a4a694dd87
commit 459b76f9fd
14 changed files with 935 additions and 1129 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+2 -2
View File
@@ -613,7 +613,7 @@ void Ekf::controlGpsFusion()
if (_mag_inhibit_yaw_reset_req) {
_mag_inhibit_yaw_reset_req = false;
// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
setDiag(P, 12, 12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
}
}
@@ -862,7 +862,7 @@ void Ekf::controlHeightSensorTimeouts()
const baroSample &baro_init = _baro_buffer.get_newest();
bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P[9][9]) * sq(_params.baro_innov_gate);
bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9,9)) * sq(_params.baro_innov_gate);
// if baro data is acceptable and GPS data is inaccurate, reset height to baro
bool reset_to_baro = baro_data_consistent && baro_data_fresh && !_baro_hgt_faulty && !gps_hgt_accurate;