Skip to content
This repository has been archived by the owner on May 1, 2024. It is now read-only.

Centralize pos and vel reset (use request flag) #927

Open
garlinplus opened this issue Oct 30, 2020 · 1 comment
Open

Centralize pos and vel reset (use request flag) #927

garlinplus opened this issue Oct 30, 2020 · 1 comment

Comments

@garlinplus
Copy link

In ecl ekf code, there are so many calling of reset position and vel. But in reset position and reset velocity function,the related control status flag in them.Their code as followed:
void 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) {
	// this reset is only called if we have new gps data at the fusion time horizon
	resetHorizontalPositionToGps();

} else if (_control_status.flags.ev_pos) {
	// this reset is only called if we have new ev data at the fusion time horizon
	resetHorizontalPositionToVision();

} else if (_control_status.flags.opt_flow) {
	ECL_INFO_TIMESTAMPED("reset position to last known position");

	if (!_control_status.flags.in_air) {
		// we are likely starting OF for the first time so reset the horizontal position
		resetHorizontalPositionTo(Vector2f(0.f, 0.f));
	} else {
		resetHorizontalPositionTo(_last_known_posNE);
	}

	// estimate is relative to initial position in this mode, so we start with zero error.
	P.uncorrelateCovarianceSetVariance<2>(7, 0.0f);

} else {
	ECL_INFO_TIMESTAMPED("reset position to last known position");
	// Used when falling back to non-aiding mode of operation
	resetHorizontalPositionTo(_last_known_posNE);
	P.uncorrelateCovarianceSetVariance<2>(7, sq(_params.pos_noaid_noise));
}

}

void Ekf::resetVelocity()
{
if (_control_status.flags.gps && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
// this reset is only called if we have new gps data at the fusion time horizon
resetVelocityToGps();

} else if (_control_status.flags.opt_flow) {
	resetHorizontalVelocityToOpticalFlow();

} else if (_control_status.flags.ev_vel) {
	resetVelocityToVision();

} else {
	resetHorizontalVelocityToZero();
}

}

Should we delete all positon and velocity reset in sensor fusion control function.And we define one position reset request and one velocity reset request flag variable.When the corresponding request is set,we call resetVelocity or resetHorizontalPosition function.

@priseborough
Copy link
Collaborator

Yes, this is a preferred method. We have started moving to a centralised reset request flag for some observation types such as yaw, but not yet for velocity and position.

@bresch bresch changed the title reset position and vel Centralize pos and vel reset (use request flag) Aug 20, 2021
Sign up for free to subscribe to this conversation on GitHub. Already have an account? Sign in.
Projects
None yet
Development

No branches or pull requests

2 participants