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

Update time of last fusion event really only if fused

Browse Source 
Except for the velpos fusion
This commit is contained in:
kamilritz 2020-07-03 17:09:10 +02:00 committed by Mathieu Bresciani
parent efd96db49d
commit 05855b7fc1
3 changed files with 4 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/airspeed_fusion.cpp
View File

@ -159,9 +159,6 @@ void Ekf::fuseAirspeed()
_innov_check_fail_status.flags.reject_airspeed = false;
}
// Airspeed measurement sample has passed check so record it
_time_last_arsp_fuse = _time_last_imu;
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
@ -211,6 +208,7 @@ void Ekf::fuseAirspeed()
// apply the state corrections
fuse(Kfusion, _airspeed_innov);
_time_last_arsp_fuse = _time_last_imu;
}
}
}

2
EKF/gps_yaw_fusion.cpp
View File

@ -250,7 +250,6 @@ void Ekf::fuseGpsYaw()
}
if (healthy) {
_time_last_gps_yaw_fuse = _time_last_imu;
// apply the covariance corrections
P -= KHP;
@ -259,6 +258,7 @@ void Ekf::fuseGpsYaw()
// apply the state corrections
fuse(Kfusion, _heading_innov);
_time_last_gps_yaw_fuse = _time_last_imu;
}
}

5
EKF/sideslip_fusion.cpp
View File

@ -192,9 +192,6 @@ void Ekf::fuseSideslip()
_innov_check_fail_status.flags.reject_sideslip = false;
}
// synthetic sideslip measurement sample has passed check so record it
_time_last_beta_fuse = _time_last_imu;
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
@ -249,6 +246,8 @@ void Ekf::fuseSideslip()
// apply the state corrections
fuse(Kfusion, _beta_innov);
_time_last_beta_fuse = _time_last_imu;
}
}
}