Optical flow degrades altitude hold

[RomanBapst]

@priseborough
@DanielePettenuzzo and I noticed that when setting the aid mask to 'optical flow' the altitude hold performance degrades considerably.
We did two tests (see logs and videos below) in which we flew in altitude hold mode, once with optical flow aid mask enabled and once without. As soon as you start moving horizontally the vehicle loses altitude.
Without the optical flow enabled this issues does not occur.

Optical flow disabled in aid mask:
Log: https://logs.px4.io/plot_app?log=149a1718-a893-40d2-9416-94156654bbe8
Video: https://www.youtube.com/watch?v=gnXhvMrfrrg&feature=youtu.be

Optical flow enabled in aid mask:
Log: https://logs.px4.io/plot_app?log=4486cfba-21d7-45bc-adbf-fd5b87d05a31
https://www.youtube.com/watch?v=WUxe84Mh_P8&feature=youtu.be

[mhkabir]

Nice observation. I saw this too, but didn't connect it to optical flow use. What flow sensor and lidar are you using here?

[mhkabir]

Potentially related to the height limiter which is only active when flow fusion mask is enabled.

[RomanBapst]

@priseborough I started looking into this a bit and found that something causes the height state covariance to decrease during horizontal maneuvers. I replayed the bad log (with and without flow aid mask) and noticed that this causes the kalman gain of the height state during a baro update to be cut to half. I assume that the height solution is pulled less towards the baro and height performance degrades.
Here's the plot of the height state covariance when flow is disabled:

Here's the height state covariance when optical flow is enabled:

[RomanBapst]

@priseborough Could you let me know if replaying the posted logs works for you? I can replay them but the replay log is very short (maybe one second).

[priseborough]

We need the logging to start from boot to do a useful replay. Otherwise it is doing an in-flight alignment and results are not representative.

[priseborough]

Inspecting the logs indicates we need to investigate (using replay) the possibility that optical flow observations are causing a reduction in vertical velocity variance which is also reducing vertical position variance.

[RomanBapst]

@priseborough I can provide replay logs tomorrow morning. I tried to set the rows/columns corresponding to vertical velocity and height of the KHP matrix to zero but that did not seem to have an influence. So I'm not quite sure yet how that coupling works. Anyway, replay should help us there.

[priseborough]

The theory is that as the vehicle tilts, the vertical velocity becomes observable via the angular rate corrected flow rates. If required, this behaviour can be modified by zeroing terms in the H matrix.

Edit: Anyway it looks like you already tried that. The replay should show us which fusion operation is responsible for the reduction in vertical state variances

[RomanBapst]

@priseborough I did an outdoor flight today in which I zeroed the entry of the optical flow observation vector corresponding to the vertical velocity state. The logs shows that this gets rid of the dip in vertical velocity and height state variance during horizontal maneuvers. During the flight I was not convinced of the success of the fix because the drone was still losing a lot of altitude during these maneuvers. However, after closer inspection of the logs I noticed that the barometer of the Pixhawk 4 must have been influenced a lot by dynamic pressure changes during the maneuver. The log contains the data of the distance sensor which served as a good measure for ground truth.
I also did a replay of the log without the changes to the observation matrix and the dips in variance are noticeable again.
I'm a bit unsure if we need this fix or not. Have you observed this problem on one of your drones before?

Here's a plot of height- and height rate variance with the fix:

Here's a plot of height- and height rate variance without the fix:

This is the corresponding replay log file:
https://logs.px4.io/plot_app?log=d9f72146-0bf9-4f54-92e5-7f8f3b4527e5

[priseborough]

The reduction in height variance not significant enough to account for the error, but does confirm optical flow observations are modifying the vertical states. If there is roll/pitch estimation error or angular misalignment of the flow sensor wrt the IMU, then when the vehicle is tilted, there will be vertical velocity errors introduced when optical flow data is fused.

I have seen increased height estimation errors when using optical flow on other vehicles, but that was in comparison to use of GPS and was explained at the time as being due to the loss of vertical velocity observations.

The 'fix' is a bit of a hack, but may be necessary unless we can find a tuning compromise.

[priseborough]

I've been running replay tests on https://logs.px4.io/plot_app?log=d9f72146-0bf9-4f54-92e5-7f8f3b4527e5 using PX4/Firmware master and blocking the optical flow fusion from modifying the vertical velocity and position states increased the RMS innovation levels for the baro observations.

[RomanBapst]

@priseborough Hm, what is your conclusion from that?

[RomanBapst]

@priseborough I think I found the mistake I made. I forgot that the kalman gain was computed independently of the H_LOS matrix. I zeroed the entries in the H_LOS matrix but not in the kalman gain. Replay shows promising results.

[priseborough]

I don't know what to make of the data from replay of that log at this point in time. Yes, zeroing the rows does remove the fluctuation in vertical state variances, but if I was to assess the effect purely on the innovation sequence, the estimator performance was degraded. However making an assessment based on innovation levels on their own when there are baro errors present is not reliable.

It is possible vertical velocity errors are causing more of the height fluctuation in the controller, so tomorrow the vertical velocity state. looking at the GPS vs estimated vertical velocity and also vertical position state vs range finder height when assessing the effect of replay code changes.

[RomanBapst]

@priseborough I think the replay log I provided you with is not very good. I'll provide something better to assess the potential improvement. We have also installed a traditional Px4 flow module just to verify that we are not suffering from issues related to the bitcraze flow module. At the moment we are especially struggling with effects of yawing on the flow innovations.

[RomanBapst]

@priseborough During our investigations we discovered two potential bugs for which I will provide a fix:

1. Here we are using _flow_sample_delayed.gyroXYZ which is a delta angle but we should be using
   the equivalent rate.
   https://github.com/PX4/ecl/blob/master/EKF/optflow_fusion.cpp#L81

2. The corrected height above ground estimate here is not necessarily the distance from the focal point to the center of the image. Since we know the position of the optical flow and the range finder we can correct for this.
   https://github.com/PX4/ecl/blob/master/EKF/optflow_fusion.cpp#L90

[RomanBapst]

Upper issues are addressed in #482

[RomanBapst]

@mhkabir You said that you also had this problem on one of your setups, correct? Do you have a video or a log of that.
I'm looking for other people who are seeing this issue.

[priseborough]

The pmw3901 had a wider fov and violates the assumption of the observation equations that the flow is measured at a single point that is the intersection of the Z body axis and flat terrain.

[priseborough]

That will introduce vertical estimation errors at higher tilt angles.

[mhkabir]

@priseborough Is there a way forward to fix this?

[priseborough]

There are a couple of options that come to mind:

1. If we know the effective FOV (angular region used to calculate flow vectors, could be smaller than sensor total FOV), if we assume a flat earth, the mean 'stare point' can be adjusted with tilt angle.

2. The optical flow observations can be prevented from modifying the vertical velocity states by setting the H[0][6] observation Jacobian to zero.

https://github.com/PX4/ecl/blob/master/EKF/optflow_fusion.cpp#L133
https://github.com/PX4/ecl/blob/master/EKF/optflow_fusion.cpp#L279

Method 1) will take longer requires more testing to characterise the sensor. Method 2) can be implemented quickly and is what we should try first.

@RomanBapst Do you have a log suitable for replay that shows the problem? It will be required to verify the diagnosis and test the fix.

[priseborough]

Covariance stability changes may have reduced this effect, however the current observation model assumes a pinhole camera with zero fov, aligned with Z and looking at flat terrain. The original PX4 flow sensor had a narrower fov compared to PMW390x sensors, so we may be seeing the result of the wider fov. More testing required,