FlowHold altitude estimate is incorrect - causes unstable flight

[RickReeser]

Bug report

Issue details

FlowHold depends on a height estimate in order to scale the flow measurements into velocity. It gets copter.inertial_nav altitude and then constrains it within height_min and height_max (are 0.1 and 3 meters, respectively) when it is used. https://github.com/ArduPilot/ardupilot/blob/9eea14054e624ac4b4797398be3cc21eb8fbebbc/ArduCopter/mode_flowhold.cpp#L384 https://github.com/ArduPilot/ardupilot/blob/9eea14054e624ac4b4797398be3cc21eb8fbebbc/ArduCopter/mode_flowhold.cpp#L145

However, the inav altitude is not a good number to use for height above ground level, as it is based on the EKF origin, which can be very different than ground level or home altitude. For example, in the log I provided below, my inav altitude was negative while my drone was hovering 5 meters above the ground (check CTUN.Alt or FHLD.Hest vs. CTUN.SAlt or NKF5.HAGL). This resulted in FlowHold basically doing nothing because its altitude-based flow scalar was too small. The drone just drifted with the wind as if it was in Alt Hold.

Later in the log, I land and take off again in FlowHold, which then begins to operate correctly because the flowhold height estimator starts at ~0 and increases as my drone ascends.

I'm not sure what would be best to initialize flowhold with. EKF's height above ground level might be appropriate, as I think it is our best guess for what flowhold needs to scale the flow readings correctly. I'm not sure if EKF HAGL takes terrain into account when that's enabled; if it doesn't, we should probably include that as well. However, it does not take obstacles into account, such as when flying over buildings or trees, which may be important for flow readings.

Another option is using the rangefinder, if rangefinder_ok is true. This could account for obstacles, but may be unreliable for that same reason; initializing the height estimate while flying over a tree may be a bad idea if the drone then moves over open terrain.

Also, while we're on the subject, I wonder if clamping the height estimate used in flow calculations to be a maximum of 3 meters (height_max) is appropriate for flowhold. This works for optiflow, since it's generally used indoors or at a very low altitude, but flowhold has the potential to be used at any altitude, which is important for dealing with GPS failures.

Version ArduCopter 3.6.7

Platform [ ] All [ ] AntennaTracker [ x ] Copter [ ] Plane [ ] Rover [ ] Submarine

Airframe type Quad

Hardware type Pixhawk 2.1 cube black

Logs flowhold drifting bad height est.zip

[rmackay9]

thanks for the report and investigation. I wonder if it might be a good idea to always try and run the optical flow based height estimate regardless of whether we are in flowhold mode or not. This would resolve the initialisation problem because we could assume it was zero before takeoff.

[RickReeser]

I'm changing the name of the issue because I found that the altitude estimate used by flowhold is generally just incorrect. Dangerously so.

There are two problems:

1. Using INS altitude to initialize FlowHold mode
2. The Optical flow altitude limit height_max = 3 is not appropriate for FlowHold

For the first problem, INS altitude can basically by any number, even when the drone is sitting on the ground. This leads to very incorrect altitude estimates consumed by flowhold; if the altitude is too high, the drone will oscillate unstably when near the ground (dangerous!). If it is too low, flowhold basically does nothing.

The second problem means that the optical flow measurements will never scale properly above 3 meters altitude. I tested this by flying high (20-50 meters) in flowhold mode. It was a windy day, and once above 10 meters or so, the drone began to drift away with the wind because the control effort was not sufficient to hold position. It got worse at higher altitudes.

The following log includes an unstable flight at low altitude in the first half, and drifting away at high altitude in the second half. flowhold unstable and drift.zip

[anbello]

@rmackay9 do you think this issue should be added to the Copter 3.6.0 issues list? I did some indoor test in flowhold without rangefinder and I see oscillation in altitude as reported by @RickReeser even if the situation get better using EKF3. When I have more formalized results (graphs and logs) I will post here.

[rmackay9]

@anbello, we could add it to the list but practically speaking I don't think it's very likely that we will resolve it for Copter-3.6. I can't speak for other developers but at least for me I don't think I can dig into figuring out the issues with the altitude estimate ahead of 3.7. Higher priority for me is improving object avoidance..

[RickReeser]

I am continuing to test this flight mode, and have a few more suggestions/notes:

1. As Randy suggested, the height estimator should run in all flight modes. This will help cover the case of switching to FlowHold while over terrain or building that is not the same altitude as the home location.
2. The height estimator has a strong bias towards lower altitudes, and this has caused problems for me in a few flights. It will especially be problematic if the estimator is running for a long time. I will continue to test this.

[rmackay9]

@RickReeser, great, thanks. very helpful.

I've also have noticed the tendency to estimate a very low altitude. when I tested with the OpenMV camera it would very quickly get to an incredibly low altitude estimate (like 50cm or less). I suspect it's not handling the delay from the camera properly. when comparing the flow data to the IMUs it should take into account the sensor delay and I think it doesn't.

[RickReeser]

I tried changing optiflow's max_height to 99 so that it should work at higher altitudes. This exposed two problems:

1. After descending from higher altitudes in another flight mode, switching to FlowHold causes a severe twitch. Probably related to a residual altitude value being used. This will probably go away if the altitude estimator is always running, but may warrant a look at how altitude is used when we switch to flowhold. I marked these events in the graph below with a blue circle.
2. Flowhold oscillates unstably above 6 or 7 meters. These are marked with a blue underline. You can see the roll twitches getting more severe as I ascend, eventually becoming an unstable oscillation. Higher altitudes makes it worse. I guess there might be something wrong with how flow data scales with altitude, but I'm having trouble interpreting the data.

Your observation that the sensor lag is not being accounted for is interesting. Maybe this high altitude oscillation is another symptom? That seems to make intuitive sense: at high altitudes, flow data is strongly scaled when converting it to translational movement, so if the body-frame rotation isn't being subtracted from the flow data in time, it would be momentarily interpreted as large translational movement.

Here's the log: flowhold_maxheight_99.zip

[RickReeser]

Just flew using optical flow and no GPS in Loiter (EK2_GPS_TYPE=3) with my max_height = 99 change. Even at 50+ meters altitude, the flight behavior was very stable and generally well-behaved. The GPS was still plugged in, so we can compare it to the flow odometry, which looks pretty darn good (there's a part where flying over trees confused it a bit):

It makes me wonder if FlowHold really needs to be any different than optiflow-Loiter. The height estimator is necessary if we can't depend on a rangefinder, and I understand that odometry may not be accurate enough for navigation at high altitude, but this makes me wonder what advantages FlowHold offers over Loiter for just position holding with optical flow.

[rmackay9]

@RickReeser, nice. I think the only advantage of FlowHold over Loiter is that it doesn't require the rangefinder. If the vehicle has a rangefinder I don't think there's a good reason to use it.