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

Refactor velocity resets

Browse Source
This commit is contained in:
Kamil Ritz
2020-05-12 08:05:32 +02:00
committed by Mathieu Bresciani
parent 03191847f9
commit b40adf3dec
2 changed files with 63 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.h
+8
View File
@@ -574,6 +574,14 @@ private:
// reset velocity states of the ekf // reset velocity states of the ekf
bool resetVelocity(); bool resetVelocity();
void resetVelocityToGps();
void resetHorizontalVelocityToOpticalFlow();
void resetVelocityToVision();
void resetHorizontalVelocityToZero();
void resetVelocityTo(const Vector3f &vel); void resetVelocityTo(const Vector3f &vel);
inline void resetHorizontalVelocityTo(const Vector2f &new_horz_vel); inline void resetHorizontalVelocityTo(const Vector2f &new_horz_vel);
EKF/ekf_helper.cpp
+55 -40
View File
@@ -50,58 +50,73 @@
bool Ekf::resetVelocity() bool Ekf::resetVelocity()
{ {
if (_control_status.flags.gps && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) { if (_control_status.flags.gps && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
ECL_INFO_TIMESTAMPED("reset velocity to GPS");
// this reset is only called if we have new gps data at the fusion time horizon // this reset is only called if we have new gps data at the fusion time horizon
resetVelocityTo(_gps_sample_delayed.vel); resetVelocityToGps();
P.uncorrelateCovarianceSetVariance<3>(4, sq(_gps_sample_delayed.sacc));
} else if (_control_status.flags.opt_flow) { } else if (_control_status.flags.opt_flow) {
ECL_INFO_TIMESTAMPED("reset velocity to flow"); resetHorizontalVelocityToOpticalFlow();
// constrain height above ground to be above minimum possible
const float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
// calculate absolute distance from focal point to centre of frame assuming a flat earth
const float range = heightAboveGndEst / _range_sensor.getCosTilt();
if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
// we should have reliable OF measurements so
// calculate X and Y body relative velocities from OF measurements
Vector3f vel_optflow_body;
vel_optflow_body(0) = - range * _flow_compensated_XY_rad(1) / _flow_sample_delayed.dt;
vel_optflow_body(1) = range * _flow_compensated_XY_rad(0) / _flow_sample_delayed.dt;
vel_optflow_body(2) = 0.0f;
// rotate from body to earth frame
const Vector3f vel_optflow_earth = _R_to_earth * vel_optflow_body;
resetHorizontalVelocityTo(Vector2f(vel_optflow_earth));
} else {
resetHorizontalVelocityTo(Vector2f{0.f, 0.f});
}
// reset the horizontal velocity variance using the optical flow noise variance
P.uncorrelateCovarianceSetVariance<2>(4, sq(range) * calcOptFlowMeasVar());
} else if (_control_status.flags.ev_vel) { } else if (_control_status.flags.ev_vel) {
ECL_INFO_TIMESTAMPED("reset velocity to ev velocity"); resetVelocityToVision();
Vector3f _ev_vel = _ev_sample_delayed.vel;
if(_params.fusion_mode & MASK_ROTATE_EV){
_ev_vel = _R_ev_to_ekf *_ev_sample_delayed.vel;
}
resetVelocityTo(_ev_vel);
P.uncorrelateCovarianceSetVariance<3>(4, _ev_sample_delayed.velVar);
} else { } else {
ECL_INFO_TIMESTAMPED("reset velocity to zero"); resetHorizontalVelocityToZero();
// Used when falling back to non-aiding mode of operation
resetHorizontalVelocityTo(Vector2f{0.f, 0.f});
P.uncorrelateCovarianceSetVariance<2>(4, 25.0f);
} }
return true; return true;
} }
void Ekf::resetVelocityToGps() {
ECL_INFO_TIMESTAMPED("reset velocity to GPS");
resetVelocityTo(_gps_sample_delayed.vel);
P.uncorrelateCovarianceSetVariance<3>(4, sq(_gps_sample_delayed.sacc));
}
void Ekf::resetHorizontalVelocityToOpticalFlow() {
ECL_INFO_TIMESTAMPED("reset velocity to flow");
// constrain height above ground to be above minimum possible
const float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
// calculate absolute distance from focal point to centre of frame assuming a flat earth
const float range = heightAboveGndEst / _range_sensor.getCosTilt();
if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
// we should have reliable OF measurements so
// calculate X and Y body relative velocities from OF measurements
Vector3f vel_optflow_body;
vel_optflow_body(0) = - range * _flow_compensated_XY_rad(1) / _flow_sample_delayed.dt;
vel_optflow_body(1) = range * _flow_compensated_XY_rad(0) / _flow_sample_delayed.dt;
vel_optflow_body(2) = 0.0f;
// rotate from body to earth frame
const Vector3f vel_optflow_earth = _R_to_earth * vel_optflow_body;
resetHorizontalVelocityTo(Vector2f(vel_optflow_earth));
} else {
resetHorizontalVelocityTo(Vector2f{0.f, 0.f});
}
// reset the horizontal velocity variance using the optical flow noise variance
P.uncorrelateCovarianceSetVariance<2>(4, sq(range) * calcOptFlowMeasVar());
}
void Ekf::resetVelocityToVision() {
ECL_INFO_TIMESTAMPED("reset to vision velocity");
Vector3f _ev_vel = _ev_sample_delayed.vel;
if(_params.fusion_mode & MASK_ROTATE_EV){
_ev_vel = _R_ev_to_ekf *_ev_sample_delayed.vel;
}
resetVelocityTo(_ev_vel);
P.uncorrelateCovarianceSetVariance<3>(4, _ev_sample_delayed.velVar);
}
void Ekf::resetHorizontalVelocityToZero() {
ECL_INFO_TIMESTAMPED("reset velocity to zero");
// Used when falling back to non-aiding mode of operation
resetHorizontalVelocityTo(Vector2f{0.f, 0.f});
P.uncorrelateCovarianceSetVariance<2>(4, 25.0f);
}
void Ekf::resetVelocityTo(const Vector3f &new_vel) { void Ekf::resetVelocityTo(const Vector3f &new_vel) {
resetHorizontalVelocityTo(Vector2f(new_vel)); resetHorizontalVelocityTo(Vector2f(new_vel));
resetVerticalVelocityTo(new_vel(2)); resetVerticalVelocityTo(new_vel(2));