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

Apply measurementUpdate function

Browse Source
This commit is contained in:
kamilritz
2020-08-22 10:24:03 +02:00
committed by Paul Riseborough
parent 404edde7f3
commit 644e903552
6 changed files with 22 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/airspeed_fusion.cpp
+3 -17
View File
@@ -152,26 +152,12 @@ void Ekf::fuseAirspeed()
_innov_check_fail_status.flags.reject_airspeed = false; _innov_check_fail_status.flags.reject_airspeed = false;
} }
// apply covariance correction via P_new = (I -K*H)*P const bool is_fused = measurementUpdate(Kfusion, Hfusion, _airspeed_innov);
// first calculate expression for KHP
// then calculate P - KHP
const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion);
const bool healthy = checkAndFixCovarianceUpdate(KHP); _fault_status.flags.bad_airspeed = !is_fused;
_fault_status.flags.bad_airspeed = !healthy;
if (healthy) {
// apply the covariance corrections
P -= KHP;
fixCovarianceErrors(true);
// apply the state corrections
fuse(Kfusion, _airspeed_innov);
if (is_fused) {
_time_last_arsp_fuse = _time_last_imu; _time_last_arsp_fuse = _time_last_imu;
} }
} }
EKF/drag_fusion.cpp
+1 -17
View File
@@ -264,23 +264,7 @@ void Ekf::fuseDrag()
// if the innovation consistency check fails then don't fuse the sample // if the innovation consistency check fails then don't fuse the sample
if (_drag_test_ratio(axis_index) <= 1.0f) { if (_drag_test_ratio(axis_index) <= 1.0f) {
// apply covariance correction via P_new = (I -K*H)*P measurementUpdate(Kfusion, Hfusion, _drag_innov(axis_index));
// first calculate expression for KHP
// then calculate P - KHP
const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion);
const bool healthy = checkAndFixCovarianceUpdate(KHP);
if (healthy) {
// apply the covariance corrections
P -= KHP;
fixCovarianceErrors(true);
// apply the state corrections
fuse(Kfusion, _drag_innov(axis_index));
}
} }
} }
} }
EKF/gps_yaw_fusion.cpp
+3 -17
View File
@@ -186,24 +186,10 @@ void Ekf::fuseGpsYaw()
Kfusion(row) = HK32*(-HK16*P(0,row) - HK24*P(1,row) - HK25*P(2,row) + HK26*P(3,row)); Kfusion(row) = HK32*(-HK16*P(0,row) - HK24*P(1,row) - HK25*P(2,row) + HK26*P(3,row));
} }
// apply covariance correction via P_new = (I -K*H)*P const bool is_fused = measurementUpdate(Kfusion, Hfusion, _heading_innov);
// first calculate expression for KHP _fault_status.flags.bad_hdg = !is_fused;
// then calculate P - KHP
const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion);
const bool healthy = checkAndFixCovarianceUpdate(KHP);
_fault_status.flags.bad_hdg = !healthy;
if (healthy) {
// apply the covariance corrections
P -= KHP;
fixCovarianceErrors(true);
// apply the state corrections
fuse(Kfusion, _heading_innov);
if (is_fused) {
_time_last_gps_yaw_fuse = _time_last_imu; _time_last_gps_yaw_fuse = _time_last_imu;
} }
} }
EKF/mag_fusion.cpp
+8 -37
View File
@@ -399,33 +399,19 @@ void Ekf::fuseMag()
Kfusion(21) = HKZ23*HKZ24; Kfusion(21) = HKZ23*HKZ24;
} }
// apply covariance correction via P_new = (I -K*H)*P const bool is_fused = measurementUpdate(Kfusion, Hfusion, _mag_innov(index));
// first calculate expression for KHP
// then calculate P - KHP
const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion);
const bool healthy = checkAndFixCovarianceUpdate(KHP);
if (index == 0) { if (index == 0) {
_fault_status.flags.bad_mag_x = !healthy; _fault_status.flags.bad_mag_x = !is_fused;
} else if (index == 1) { } else if (index == 1) {
_fault_status.flags.bad_mag_y = !healthy; _fault_status.flags.bad_mag_y = !is_fused;
} else if (index == 2) { } else if (index == 2) {
_fault_status.flags.bad_mag_z = !healthy; _fault_status.flags.bad_mag_z = !is_fused;
} }
if (healthy) { if (is_fused) {
// apply the covariance corrections
P -= KHP;
fixCovarianceErrors(true);
// apply the state corrections
fuse(Kfusion, _mag_innov(index));
// constrain the declination of the earth field states
limitDeclination(); limitDeclination();
} }
} }
@@ -926,26 +912,11 @@ void Ekf::fuseDeclination(float decl_sigma)
const float innovation = math::constrain(atan2f(magE, magN) - getMagDeclination(), -0.5f, 0.5f); const float innovation = math::constrain(atan2f(magE, magN) - getMagDeclination(), -0.5f, 0.5f);
// apply covariance correction via P_new = (I -K*H)*P const bool is_fused = measurementUpdate(Kfusion, Hfusion, innovation);
// first calculate expression for KHP
// then calculate P - KHP
// take advantage of the empty columns in KH to reduce the number of operations
const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion);
const bool healthy = checkAndFixCovarianceUpdate(KHP); _fault_status.flags.bad_mag_decl = !is_fused;
_fault_status.flags.bad_mag_decl = !healthy; if (is_fused) {
if (healthy) {
// apply the covariance corrections
P -= KHP;
fixCovarianceErrors(true);
// apply the state corrections
fuse(Kfusion, innovation);
// constrain the declination of the earth field states
limitDeclination(); limitDeclination();
} }
} }
EKF/optflow_fusion.cpp
+4 -17
View File
@@ -312,29 +312,16 @@ void Ekf::fuseOptFlow()
} }
// apply covariance correction via P_new = (I -K*H)*P const bool is_fused = measurementUpdate(Kfusion, Hfusion, _flow_innov(obs_index));
// first calculate expression for KHP
// then calculate P - KHP
const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion);
const bool healthy = checkAndFixCovarianceUpdate(KHP);
if (obs_index == 0) { if (obs_index == 0) {
_fault_status.flags.bad_optflow_X = !healthy; _fault_status.flags.bad_optflow_X = !is_fused;
} else if (obs_index == 1) { } else if (obs_index == 1) {
_fault_status.flags.bad_optflow_Y = !healthy; _fault_status.flags.bad_optflow_Y = !is_fused;
} }
if (healthy) { if (is_fused) {
// apply the covariance corrections
P -= KHP;
fixCovarianceErrors(true);
// apply the state corrections
fuse(Kfusion, _flow_innov(obs_index));
_time_last_of_fuse = _time_last_imu; _time_last_of_fuse = _time_last_imu;
} }
} }
EKF/sideslip_fusion.cpp
+3 -16
View File
@@ -203,24 +203,11 @@ void Ekf::fuseSideslip()
Kfusion(22) = HK45*HK52; Kfusion(22) = HK45*HK52;
Kfusion(23) = HK42*HK52; Kfusion(23) = HK42*HK52;
// apply covariance correction via P_new = (I -K*H)*P const bool is_fused = measurementUpdate(Kfusion, Hfusion, _beta_innov);
// first calculate expression for KHP
// then calculate P - KHP
const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion);
const bool healthy = checkAndFixCovarianceUpdate(KHP); _fault_status.flags.bad_sideslip = !is_fused;
_fault_status.flags.bad_sideslip = !healthy;
if (healthy) {
// apply the covariance corrections
P -= KHP;
fixCovarianceErrors(true);
// apply the state corrections
fuse(Kfusion, _beta_innov);
if (is_fused) {
_time_last_beta_fuse = _time_last_imu; _time_last_beta_fuse = _time_last_imu;
} }
} }