Critical wind estimation issue

[CarlOlsson]

Issue 1:

When a VTOL transitions into fixed wing flight airspeed and synthetic sideslip fusion are activated. However, this is done in different ways. Airspeed fusion is activated by the fact that ekf2_main starts to push airspeed data into ecl when the measurements are above 7 m/s, synthetic sideslip fusion on the other hand is activated by a flag being set in ekf2_main as soon as forward transition is finished.

This logic leads to the fact that ecl will always fuse beta before airspeed, hence initializing the wind states without airspeed measurements.

To test this, run standard_vtol in SITL, set EKF2_ARSP_THR to 7 and EKF2_FUSE_BETA to 1. Now the wind states will be initialized to zero.

Issue 2:

The issue documented above wouldn't be so critical if it wasn't for the following fact:
In the airspeed fusion logic the predicted measurement is the norm of the body velocity vector minus the wind speed. If ecl starts fusing airspeed when the wind states are zero, e.g. right after forward transition due to issue 1 above, the updated wind states are:

wind(k+1) = wind(k) - KalmanGain * innovation

wind(k) is zero and the innovation is

innovation = TAS_predicted - measurement = body_velocity_norm - measurment

Lets say that the airplane is moving straight north with a ground speed of 10 m/s and the airspeed sensor is measuring 12 m/s, I.e. the wind is actually -2 m/s.

The innovation is then -2 m/s and the wind

wind(k+1) = wind(k) - KalmanGain * innovation = 0 - KalmanGain * (-2) = 2 * KalmanGain

In this case the Kalman gain would be negative and the wind state would converge to -2 m/s, I.e. wind from the north.

However, imagine the same situation but the VTOL airplane is taking off in a lot of wind. It is facing north during takeoff and performs the transition. However, due to the wind the horizontal north velocity is now slightly negative when the transition is finished and we start fusing airspeed and beta. The airspeed fusion is starting to fuse with wind states initialized to zero, due to issue 1.

wind(k+1) = wind(k) - KalmanGain * innovation

wind(k) is zero and the innovation is

innovation = TAS_predicted - measurement = body_velocity_norm - measurment

Lets say that the airplane is moving straight south (but heading north) with a ground speed of 3 m/s and the airspeed sensor is measuring 8 m/s, I.e. the wind is actually -11 m/s.

The innovation is then -5 m/s and the wind

wind(k+1) = wind(k) - KalmanGain * innovation = 0 - KalmanGain * (-5) = 5 * KalmanGain

In this case the Kalman gain is actually positive, leading to the wind states being estimated with the wrong sign and amplitude, I.e. the wind states are being estimated in the complete opposite direction of reality.

@priseborough I am happy to work on a fix for this, but would appreciate your inputs before I start implementing

[priseborough]

This is an issue that could affect conventional fixed wing as well depending on launch method used. We can use the control status to determine when wind states become active and not enable sideslip fusion if wind states are inactive and airspeed data is being received. I have submitted a draft PR that you can use as a starting point: PX4/PX4-Autopilot#8954

[CarlOlsson]

@priseborough Thanks for the initial implementation! Will look into it as soon as possible. However, in the mean time I was just wondering, why don't we change the derivation of the airspeed fusion measurement equation to take the vehicle attitude into account? I.e. why is the predicted measurement not estimated airspeed along body x instead of just the norm of the body velocity vector minus wind speed?

[priseborough]

It was done to make the method as general as possible and also sensors are not alway aligned with X in the XZ plane which can cause issues with vertical state estimation. Also there are wind speed sensors available that work in any direction which are used on helicopters. We could provide a parameter selectable option for a X axis forward sensor with an adjustable pitch offset.

[priseborough]

That would then be the default setting given most applications for airspeed sensing are fixed wing / VTOL platforms.

[CarlOlsson]

That would be awesome! How do you want to tackle it, should I create a prototype or do you want to start with a branch?

[priseborough]

I would start with a wip branch and add another section to the derivation so we retain the exisiting method, eg:

%% derive equations for fusion of true airspeed measurements for forward aligned sensor
load('StatePrediction.mat');
VtasPred = ......; % predicted measurement
H_TASX = jacobian(VtasPred,stateVector); % measurement Jacobian
[H_TASX,SH_TASX]=OptimiseAlgebra(H_TASX,'SH_TASX'); % optimise processing
K_TASX = (Ptranspose(H_TASX))/(H_TASXP*transpose(H_TASX) + R_TAS);
[K_TASX,SK_TASX]=OptimiseAlgebra(K_TASX,'SK_TASX'); % Kalman gain vector

etc

[CarlOlsson]

Okey yes sounds like a good idea. Will work on it tomorrow, tell me if you start something before then.

[CarlOlsson]

Here is a first wip branch https://github.com/PX4/ecl/tree/wip-new_arsp_fusion
To begin with I disregarded the adjustable pitch offset and it seems to work so far. However, I just realized that the predicted measurement shouldn't be allowed to be negative. The Matlab symbolic toolbox doesn't seem to support the max function, @priseborough do you know a way around this?

[priseborough]

You will have to look after that in the implementation. There is a way to do that remembering that the partial derivative should still be correct even if the prediction is negative.

Can we focus on the logic protection in ekf2_main first and work on the derivation as a longer term task? If this is a critical issue then we need the logging protection now and adding new fusion methods takes time.

[priseborough]

Not present in current ecl master