forked from Archive/PX4-Autopilot
ekf2: move vel/pos reset helpers
This commit is contained in:
Daniel Agar
parent
22b957696d
commit
8243b4f474
|
|
@ -45,121 +45,6 @@
|
|||
#include <lib/world_magnetic_model/geo_mag_declination.h>
|
||||
#include <cstdlib>
|
||||
|
||||
void Ekf::resetHorizontalVelocityToZero()
|
||||
{
|
||||
_information_events.flags.reset_vel_to_zero = true;
|
||||
ECL_INFO("reset velocity to zero");
|
||||
// Used when falling back to non-aiding mode of operation
|
||||
resetHorizontalVelocityTo(Vector2f{0.f, 0.f}, 25.f);
|
||||
}
|
||||
|
||||
void Ekf::resetVelocityTo(const Vector3f &new_vel, const Vector3f &new_vel_var)
|
||||
{
|
||||
resetHorizontalVelocityTo(Vector2f(new_vel), Vector2f(new_vel_var(0), new_vel_var(1)));
|
||||
resetVerticalVelocityTo(new_vel(2), new_vel_var(2));
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var)
|
||||
{
|
||||
const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel);
|
||||
_state.vel.xy() = new_horz_vel;
|
||||
|
||||
if (PX4_ISFINITE(new_horz_vel_var(0))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0)));
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(new_horz_vel_var(1))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1)));
|
||||
}
|
||||
|
||||
_output_predictor.resetHorizontalVelocityTo(delta_horz_vel);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) {
|
||||
_state_reset_status.velNE_change = delta_horz_vel;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.velNE_change += delta_horz_vel;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.velNE++;
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hor_vel_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
|
||||
{
|
||||
const float delta_vert_vel = new_vert_vel - _state.vel(2);
|
||||
_state.vel(2) = new_vert_vel;
|
||||
|
||||
if (PX4_ISFINITE(new_vert_vel_var)) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var));
|
||||
}
|
||||
|
||||
_output_predictor.resetVerticalVelocityTo(delta_vert_vel);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) {
|
||||
_state_reset_status.velD_change = delta_vert_vel;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.velD_change += delta_vert_vel;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.velD++;
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_ver_vel_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionToLastKnown()
|
||||
{
|
||||
ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
|
||||
|
||||
_information_events.flags.reset_pos_to_last_known = true;
|
||||
|
||||
// Used when falling back to non-aiding mode of operation
|
||||
resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise));
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var)
|
||||
{
|
||||
const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}};
|
||||
_state.pos.xy() = new_horz_pos;
|
||||
|
||||
if (PX4_ISFINITE(new_horz_pos_var(0))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0)));
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(new_horz_pos_var(1))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1)));
|
||||
}
|
||||
|
||||
_output_predictor.resetHorizontalPositionTo(delta_horz_pos);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
|
||||
_state_reset_status.posNE_change = delta_horz_pos;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.posNE_change += delta_horz_pos;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.posNE++;
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hor_pos_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
bool Ekf::isHeightResetRequired() const
|
||||
{
|
||||
// check if height is continuously failing because of accel errors
|
||||
|
|
@ -171,68 +56,6 @@ bool Ekf::isHeightResetRequired() const
|
|||
return (continuous_bad_accel_hgt || hgt_fusion_timeout);
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy) {
|
||||
_information_events.flags.reset_pos_to_ext_obs = true;
|
||||
ECL_INFO("reset position to external observation");
|
||||
resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy));
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
|
||||
{
|
||||
const float old_vert_pos = _state.pos(2);
|
||||
_state.pos(2) = new_vert_pos;
|
||||
|
||||
if (PX4_ISFINITE(new_vert_pos_var)) {
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var));
|
||||
}
|
||||
|
||||
const float delta_z = new_vert_pos - old_vert_pos;
|
||||
|
||||
// apply the change in height / height rate to our newest height / height rate estimate
|
||||
// which have already been taken out from the output buffer
|
||||
_output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
|
||||
_state_reset_status.posD_change = delta_z;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.posD_change += delta_z;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.posD++;
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
terrainHandleVerticalPositionReset(delta_z);
|
||||
#endif
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hgt_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalVelocityToZero()
|
||||
{
|
||||
// we don't know what the vertical velocity is, so set it to zero
|
||||
// Set the variance to a value large enough to allow the state to converge quickly
|
||||
// that does not destabilise the filter
|
||||
resetVerticalVelocityTo(0.0f, 10.f);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
|
||||
float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
|
||||
{
|
||||
|
|
|
|||
|
|
@ -108,3 +108,103 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
|
|||
aid_src.fused = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var)
|
||||
{
|
||||
const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}};
|
||||
_state.pos.xy() = new_horz_pos;
|
||||
|
||||
if (PX4_ISFINITE(new_horz_pos_var(0))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0)));
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(new_horz_pos_var(1))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1)));
|
||||
}
|
||||
|
||||
_output_predictor.resetHorizontalPositionTo(delta_horz_pos);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
|
||||
_state_reset_status.posNE_change = delta_horz_pos;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.posNE_change += delta_horz_pos;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.posNE++;
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hor_pos_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
|
||||
{
|
||||
const float old_vert_pos = _state.pos(2);
|
||||
_state.pos(2) = new_vert_pos;
|
||||
|
||||
if (PX4_ISFINITE(new_vert_pos_var)) {
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var));
|
||||
}
|
||||
|
||||
const float delta_z = new_vert_pos - old_vert_pos;
|
||||
|
||||
// apply the change in height / height rate to our newest height / height rate estimate
|
||||
// which have already been taken out from the output buffer
|
||||
_output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
|
||||
_state_reset_status.posD_change = delta_z;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.posD_change += delta_z;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.posD++;
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
terrainHandleVerticalPositionReset(delta_z);
|
||||
#endif
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hgt_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionToLastKnown()
|
||||
{
|
||||
ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
|
||||
_information_events.flags.reset_pos_to_last_known = true;
|
||||
|
||||
// Used when falling back to non-aiding mode of operation
|
||||
resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise));
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy)
|
||||
{
|
||||
ECL_INFO("reset position to external observation");
|
||||
_information_events.flags.reset_pos_to_ext_obs = true;
|
||||
|
||||
resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -114,3 +114,82 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
|
|||
aid_src.fused = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var)
|
||||
{
|
||||
const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel);
|
||||
_state.vel.xy() = new_horz_vel;
|
||||
|
||||
if (PX4_ISFINITE(new_horz_vel_var(0))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0)));
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(new_horz_vel_var(1))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1)));
|
||||
}
|
||||
|
||||
_output_predictor.resetHorizontalVelocityTo(delta_horz_vel);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.velNE == _state_reset_count_prev.velNE) {
|
||||
_state_reset_status.velNE_change = delta_horz_vel;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.velNE_change += delta_horz_vel;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.velNE++;
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hor_vel_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
|
||||
{
|
||||
const float delta_vert_vel = new_vert_vel - _state.vel(2);
|
||||
_state.vel(2) = new_vert_vel;
|
||||
|
||||
if (PX4_ISFINITE(new_vert_vel_var)) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var));
|
||||
}
|
||||
|
||||
_output_predictor.resetVerticalVelocityTo(delta_vert_vel);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.velD == _state_reset_count_prev.velD) {
|
||||
_state_reset_status.velD_change = delta_vert_vel;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.velD_change += delta_vert_vel;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.velD++;
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_ver_vel_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalVelocityToZero()
|
||||
{
|
||||
ECL_INFO("reset velocity to zero");
|
||||
_information_events.flags.reset_vel_to_zero = true;
|
||||
|
||||
// Used when falling back to non-aiding mode of operation
|
||||
resetHorizontalVelocityTo(Vector2f{0.f, 0.f}, 25.f);
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalVelocityToZero()
|
||||
{
|
||||
// we don't know what the vertical velocity is, so set it to zero
|
||||
// Set the variance to a value large enough to allow the state to converge quickly
|
||||
// that does not destabilise the filter
|
||||
resetVerticalVelocityTo(0.0f, 10.f);
|
||||
}
|
||||
|
||||
void Ekf::resetVelocityTo(const Vector3f &new_vel, const Vector3f &new_vel_var)
|
||||
{
|
||||
resetHorizontalVelocityTo(Vector2f(new_vel), Vector2f(new_vel_var(0), new_vel_var(1)));
|
||||
resetVerticalVelocityTo(new_vel(2), new_vel_var(2));
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue