[Bug] Trajectory Setpoint (horizontal velocities) reset abruptly when EKF2 switches to a healthier instance

[alistair-blueflite]

Describe the bug

We have noticed in our mission and position mode flights, quite a few times that there is an abrupt change to the horizontal velocity x,y setpoints in trajectory_setpoint uORB. Whenever, this has occurred it has been accompanied by EKF Selector choosing a different instance, while all 3 instances always report they are healthy.

In the mission mode at least, we suspect the issue is coming from '_ekfResetHandlerVelocityXY' in FlightTaskAuto.cpp, where it sets the setpoint to the current velocity.

Attached are some screenshots of the problem in one such mission.

@julianoes

To Reproduce

Fly a mission or in position mode. And if you happen to get a EKF selection change, you will likely notice an abrupt change in the horizontal x,y setpoints from trajectory_setpoint uORB.

Expected behavior

The ekf resets should be handled in a smooth manner in position and mission modes

Screenshot / Media

Chart 1 shows that around 52 seconds in, there is an abrupt change to horizontal velocity trajectory setpoints. And Chart 2 shoes that right around the same time, the EKF switches from instance 2 to instance 0, and all 3 instances are healthy

Flight Log

https://review.px4.io/plot_app?log=96b235b0-dfd8-4626-8199-51dda835c117

Software Version

custom version of 1.14 beta 2, but none of the code in EKF2 or FlightModeManger has been changed.

Flight controller

holybro 6X

Vehicle type

Hybrid VTOL

How are the different components wired up (including port information)

No response

Additional context

No response

[julianoes]

@bresch and @MaEtUgR do you have more insight into this? Is this maybe fixed in v1.15 or later?

[bresch]

@alistair-blueflite The flight task auto reacts to the EKF reset by resetting the setpoint to the current estimate:

PX4-Autopilot/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

Lines 728 to 731 in 36d89df

	void FlightTaskAuto::_ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy)
	{
	_position_smoothing.forceSetVelocity({_velocity(0), _velocity(1), NAN});
	}

Usually this is "fine" as the velocity should track the setpoint quite well and that an EKF lane switch should be a rare event. In your case, the velocity tracking error is ~2m/s and this is why you can see a large glitch.
To avoid any glitch, we should use the reset delta sent by the EKF (already transmitted as an argument in _ekfResetHandlerVelocityXY but not used).

[alistair-blueflite]

Thank you very much for the problem diagnosis and proposed solution. Is there any plan to incorporate this fix into PX4 anytime soon? Regardless, I think we will still implement your proposed fix in our custom version as of now.