EKF2: reset position by fusion (#23279)

* reset position by fusion

* handle local_pos_valid for fixed wing in gnss denied

* [WIP] ekf2: setEkfGlobalOrigin respect current height reference and vertical position aiding

* global origin, also reset vertical pos without gps as ref

* fix wo gnss, that bitcraze ci passes

* revert some changes as requested

* remove duplicate reset messages

* undo unrelated whitespace changes, I'll fix it everywhere in a followup

* [SQUASH] ekf2: add vehicle_command_ack

* resetGlobalPosToExternalObservation consolidate logic

* remove gnss check from local_pos validation check

* reset when  0<accuracy<1, otherwise fuse

* replace gps param with flag

* ekf2: dead reckon time exceeded, respect ZUPT preflight if air data or optical flow expected

* subtract timeout from last inertial dead-reck, change fake pos conditions, save flash

---------

Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Marco Hauswirth
2024-07-07 22:43:55 +02:00
committed by GitHub
co-authored by Daniel Agar
parent ac1effa32a
commit e04c53241a
10 changed files with 182 additions and 68 deletions
+53 -10
View File
@@ -281,23 +281,66 @@ void Ekf::predictState(const imuSample &imu_delayed)
_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
}
void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
{
if (!_pos_ref.isInitialized()) {
return;
ECL_WARN("unable to reset global position, position reference not initialized");
return false;
}
// apply a first order correction using velocity at the delated time horizon and the delta time
timestamp_observation = math::min(_time_latest_us, timestamp_observation);
const float dt = _time_delayed_us > timestamp_observation ? static_cast<float>(_time_delayed_us - timestamp_observation)
* 1e-6f : -static_cast<float>(timestamp_observation - _time_delayed_us) * 1e-6f;
Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg);
Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg) + _state.vel.xy() * dt;
// apply a first order correction using velocity at the delayed time horizon and the delta time
if ((timestamp_observation > 0) && (isHorizontalAidingActive() || !_horizontal_deadreckon_time_exceeded)) {
resetHorizontalPositionTo(pos_corrected, sq(math::max(accuracy, 0.01f)));
timestamp_observation = math::min(_time_latest_us, timestamp_observation);
ECL_INFO("reset position to external observation");
_information_events.flags.reset_pos_to_ext_obs = true;
float diff_us = 0.f;
if (_time_delayed_us >= timestamp_observation) {
diff_us = static_cast<float>(_time_delayed_us - timestamp_observation);
} else {
diff_us = -static_cast<float>(timestamp_observation - _time_delayed_us);
}
const float dt_s = diff_us * 1e-6f;
pos_corrected += _state.vel.xy() * dt_s;
}
const float obs_var = math::max(accuracy, sq(0.01f));
const Vector2f innov = Vector2f(_state.pos.xy()) - pos_corrected;
const Vector2f innov_var = Vector2f(getStateVariance<State::vel>()) + obs_var;
const float sq_gate = sq(5.f); // magic hardcoded gate
const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)),
sq(innov(1)) / (sq_gate * innov_var(1))};
const bool innov_rejected = (test_ratio.max() > 1.f);
if (!_control_status.flags.in_air || (accuracy > 0.f && accuracy < 1.f) || innov_rejected) {
// when on ground or accuracy chosen to be very low, we hard reset position
// this allows the user to still send hard resets at any time
ECL_INFO("reset position to external observation");
_information_events.flags.reset_pos_to_ext_obs = true;
resetHorizontalPositionTo(pos_corrected, obs_var);
_last_known_pos.xy() = _state.pos.xy();
return true;
} else {
if (fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0)
&& fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1)
) {
ECL_INFO("fused external observation as position measurement");
_time_last_hor_pos_fuse = _time_delayed_us;
_last_known_pos.xy() = _state.pos.xy();
return true;
}
}
return false;
}
void Ekf::updateParameters()