Range aid: use tilt angle not horizontal velocity

[dakejahl]

@priseborough I'd like to change range aidings dependence from horizontal_velocity to tilt_angle. I think it makes the most sense for each range sensor to publish a max_tilt in the distance_sensor_msg because some range sensors need to be normal to the surface (laser), while others are conical (sonar) and do not need to be. The dependence on horizontal_velocity seems odd, because what about a FW vehicle that is moving very fast but would still like to make use of a range sensor to support the baro?

I wanted to ask you first to get your opinion/advice before doing the work and opening a PR.

Thanks! Jake

[priseborough]

Max tilt idea is a good one.

Horizontal velocity is relevant because it exposes the EKF to terrain height variations. If someone wants to use range aid when moving fast because they are over flat ground, they can adjust the velocity threshold parameter.

[dakejahl]

I still don't understand the intent. Any variations of the terrain will be fused if they pass rangeAidConditionsMet(). The only thing I see there that will turn range aiding off (aside from velocity, sensor limits, and data continuity) is the _hagl_innov check. So assuming any changes in distance to ground are gradual (think long sloped hill) we will always fuse the range finder as long as our height estimate is within our range finder limits (and our height estimate is a function of this data!). I guess I haven't seen this because I don't do slow flights up low incline hills :laughing: but it certainly seems like this could occur (I do understand at some point the range finder will diverge significantly from gps and range aiding will eventually fail the innovation check) At faster speeds it actually seems less likely that range aiding would remain enabled because the higher frequency height variations reported from the distance sensor would either outright fail the _hagl_innov check (think flying over a picnic table) at the Ekf::fuseHagl() level or the kalman gain would become reduced enough (think sloped hill) that our measurements quickly fall outside of the window for the _hagl_innov check. Obviously once we lose range aiding these problems are gone, as our estimate follows the baro/gps and range aiding can only be regained if the distance sensor measurements fall within the window set by the _hagl_innov check.

[dakejahl]

On a similiar note, there are a few other things I'd like to tweak about the way we do range aiding.

1. Allow range aiding while landed as long as the measurements are within ground_offset +/- some_delta. I'll explain why in (2).
2. Starting from takeoff with the range sensor on, we have a very good reference for where the ground is at. As soon as we switch out of range aiding and back to the baro, reset the baros datum using the last state prediction _state_pos(2). The baro often has a bad _baro_hgt_offset due to air density variations at ground level (cool grass vs hot pavement).
3. Do not allow a switch into range aiding if the current rangefinder measurements are too far from the previous state prediction (i.e. we flew over a picnic table) or maybe we just need to improve the terrain_estimator so we can properly apply _rng_hgt_offsets in the case we flew up to the top of a cliff and decided to fly around at this new "ground level" which may be 1000ft in altitude higher

I feel it is very important that we do all we can to bring the rangefinder measurements into alignment with the barometer measurements. Any amount of DC bias between the two measurements causes poor altitude hold when riding the line between range aiding and not range aiding.

[dakejahl]

I have a few more ideas:

1. When not using GPS as primary, periodically update GPS alt_ref to bring it into alignment with the current state prediction (taking alt ref on ground is pretty unreliable due to gps multipath)
2. Update _baro_hgt_offset to bring into alignment with current state prediction when switching from range aiding back to baro.
3. In order to accomplish (1) and (2) we need to be tracking in a global variable a moving average of the measurements for baro, gps and range finder and use that as the measurement we compare against the state to set a new offset.
4. Rather than seeding the ekf local_z_pos with sensor measurements while landed we should be seeding it with 0 and then calculating our sensor offsets from there. I've seen in both our flight logs and others the local_z_pos starting at some nonsensical value such as -4 at takeoff.

Sorry for bombarding you this week, I wanted to share with you what I've discovered this week. I feel very strongly about the importance of good datums and agreement between sensors, as this ultimately effects the quality of our estimate and the consistency of flight performance.

[priseborough]

Ultimately, if we want to fude data from disparate vertical position sensing mechanisms I think we need a separate estimator to estimate the height datum offsets that incorporates knowledge of the different error mechanisms for each sensor.

[dakejahl]

I agree! I've got a few other things slated for the next few weeks, but I'd like to tackle that afterwards. Is this something you'd condone? I can do all the work but I'd like to solicit your advice during the process. Timeline is probably 6 weeks from now and I'd dedicate 2-3 weeks to the rework.

[priseborough]

OK, we have more than one issue being discussed here so I will summarise my responses:

1) I do not support removing dependence of range aid on velocity due to terrain sensitivity issue raised above. 2) I do support addition of a separate estimator to enable blending of various height sources.

If you wish to undertake 2) please raise a new issue for it (enhancement), keep the changes as modular as possible, treating the blended product as a separate height measurement type so that the changes to exisiting logic and fusion methods are minimised.