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

Refactor position resets (#822)

Browse Source
This commit is contained in:
kritz
2020-05-19 21:53:46 +02:00
committed by GitHub
parent 37d9cef262
commit 716caa5168
4 changed files with 38 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+20 -17
View File
@@ -62,7 +62,6 @@ bool Ekf::resetVelocity()
} else {
resetHorizontalVelocityToZero();
}
return true;
}
@@ -153,30 +152,18 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel) {
// Reset position states. If we have a recent and valid
// gps measurement then use for position initialisation
bool Ekf::resetPosition()
bool Ekf::resetHorizontalPosition()
{
// let the next odometry update know that the previous value of states cannot be used to calculate the change in position
_hpos_prev_available = false;
if (_control_status.flags.gps) {
ECL_INFO_TIMESTAMPED("reset position to GPS");
// this reset is only called if we have new gps data at the fusion time horizon
resetHorizontalPositionTo(Vector2f(_gps_sample_delayed.pos));
// use GPS accuracy to reset variances
P.uncorrelateCovarianceSetVariance<2>(7, sq(_gps_sample_delayed.hacc));
resetHorizontalPositionToGps();
} else if (_control_status.flags.ev_pos) {
ECL_INFO_TIMESTAMPED("reset position to ev position");
// this reset is only called if we have new ev data at the fusion time horizon
Vector3f _ev_pos = _ev_sample_delayed.pos;
if(_params.fusion_mode & MASK_ROTATE_EV){
_ev_pos = _R_ev_to_ekf *_ev_sample_delayed.pos;
}
resetHorizontalPositionTo(Vector2f(_ev_pos));
P.uncorrelateCovarianceSetVariance<2>(7, _ev_sample_delayed.posVar.slice<2, 1>(0, 0));
resetHorizontalPositionToVision();
} else if (_control_status.flags.opt_flow) {
ECL_INFO_TIMESTAMPED("reset position to last known position");
@@ -201,6 +188,22 @@ bool Ekf::resetPosition()
return true;
}
void Ekf::resetHorizontalPositionToGps() {
ECL_INFO_TIMESTAMPED("reset position to GPS");
resetHorizontalPositionTo(_gps_sample_delayed.pos);
P.uncorrelateCovarianceSetVariance<2>(7, sq(_gps_sample_delayed.hacc));
}
void Ekf::resetHorizontalPositionToVision() {
ECL_INFO_TIMESTAMPED("reset position to ev position");
Vector3f _ev_pos = _ev_sample_delayed.pos;
if(_params.fusion_mode & MASK_ROTATE_EV){
_ev_pos = _R_ev_to_ekf *_ev_sample_delayed.pos;
}
resetHorizontalPositionTo(Vector2f(_ev_pos));
P.uncorrelateCovarianceSetVariance<2>(7, _ev_sample_delayed.posVar.slice<2, 1>(0, 0));
}
void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos) {
const Vector2f delta_horz_pos = new_horz_pos - Vector2f(_state.pos);
_state.pos.xy() = new_horz_pos;
@@ -1790,7 +1793,7 @@ bool Ekf::resetYawToEKFGSF()
// reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
resetVelocity();
resetPosition();
resetHorizontalPosition();
// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
_flt_mag_align_start_time = _imu_sample_delayed.time_us;