Bug in AP_NavEKF2_PosVelFusion.cpp ?!


Thanks very much for that. So my big concern is what will happen if a user flies somewhere where the ground is not perfectly level. So so imagine this situation:

  1. set EK2_RNG_USE_HGT = 70
  2. leave RTL_ALT at the default 15m (1500cm)
  3. takeoff fly vehicle up a hill and hover about 5m above the terrain at the top (maybe the hill is 10m high so the vehicle is flying 15m above home)
  4. switch to RTL mode

When the vehicle is at the top of the hill, I think the relative alt (aka altitude above home) will change (maybe quickly, maybe slowly) and become 5m. When the vehicle enters RTL, it will then climb an additional 10m to the RTL_ALT value of 15m and then return at that altitude. This will be 25m above home which is more than the user specified.

This is just one situation where the behaviour would be unexpected. There are likely other stranger and more dangerous things that we haven’t noticed. Ah, imagine that it wasn’t a hill but rather a valley. I think the vehicle would impact the terrain on it’s way home.

Really it’s just not a good idea to be mixing up altitudes above home and altitudes above terrain. In most outdoor situations they are quite different. Only in indoor environments are they the same thing.

If that is true (and it is probably) i would consider this as a major bug in that firmware:
Alt above home should never rely on a range finder (maybe except indoors; flat ground).

So: seems that some work is required to get this correct, which means imho:

  1. Alt above Home should be derived from baro or gps
  2. Alt above terrain should be read from the rangefinder as the primary alt source, if close enough.

Imho it will climb to 15m at first, but as soon as the region of the hill is left, the baro will become the primary alt source again and it will descend to the user specified 15m


Yes, I generally agree with what you’ve said. Which is why I have added warnings in multiple places that the EK2_ALT_SOURCE should be left at 0 (baro) or maybe 2 (GPS) in some cases.

At the risk of being a bit picky, I wouldn’t go so far as to say it’s a “bug” though. I think it’s a “limitation”. The limitation being that the range finder should not be used in cases where the ground is not completely flat. If this limitation is respected then the rangefinder does give an accurate alt-above-home.

ok, agree, let’s call it limitation. But that limitation isn’t neccessary at all. Which behavior do we exspect if throttle stick = middle ?

I would like this:

If the copter is close to ground, it should follow terrain immediately by rangefinder readings. The reasons:

  1. At low altitudes, the baro is very noisy, exspec. at windy days. Todays rangefinders are very accurate.
  2. Baros tend to drift
  3. So using the rangefinder gives some safety regarding crashes

In higher altitude the copter should not follow the ground level, but maintain it’s altitude above home, or sealevel …
In 100m height, it doesn’t matter,if a baro drifts 1 or 2m…

dear @fs007, @rmackay9
i can only Comment it from the practical “in the field” side
This terrain following is in my eyes perfekt. Recently, I had to fly / do measurements at the top of a hill. I couldn’t use the Range Finder because theire were too many “winter” Trees (without Leaves) underneath.
So I went for the Google maps Data and the copter holds exactly 60 m above Terrain Level as wanted. I saw the Baro Measure was changing between 60 and 40 meters (no wind day!) and that was exactly the altitude difference from the Map Data.

When I fly in low altitudes + 2m with the Rangefinder above flat terrain, grass, or even fields the range finder Terrain following works perfect.

The only Situation, which I could complain a little bit is when Im flying my big Bird (8 x 18inch Props, TOW 12KG ) as low as 1 meter above the Ground on a gusty wind day.
Normally, without the wind, he follows the Terrain ±20cm. Even when you think about that their is a Problem with the ground effect i cant confirm that I think Im flying always in Front of the Ground effect.

But then, when their ist this gusty wind, the Problems are starting. You see the Copter jumping caused of the influence of the Baro.
At that Moment Im just wishing to have a knob of turning off the Baro and fly 100% with the RNGFND, or to have a manual mixing of Baro & Rngfnd or something like that.

But honestly, compared to the history of Altitude Hold in the past, in my eyes it is enormous where we are these Days…
2.nd But, but theire is always room for Improvement :slight_smile:


I think the current terrain following in pilot controlled modes (like Loiter) works pretty much as you describe. It will only follow the terrain if the rangefinder is in range and this range can be set using the RNGFND1_MAX_CM.

This is all done without adjusting any parameters within the EKF - the EKF doesn’t use the range finder as it’s altitude source.

So, If im changing EK2_ALT_M_NSE IT doesnt make any different to the use of the Range Finder, or? I thought that that would maybe be a possibility to give the Range finder more strenght


I have not specifically tested the EK2_ALT_M_NSE parameter when the EK2_ALT_SOURCE = 1 (i.e. rangefinder) but I strongly suspect that it will control the balance between accelerometers and range finder.

At the risk of sounding like a parrot, I’ll just re-iterate once again that the EKF’s altitude source doesn’t need to be changed in order to accomplish terrain following which is what the vast majority of users want. The EKF’s altitude source should only be changed to range finder for indoor use with a flat floor.

Thanks again for ure estimation of the situation. As I discribted above that system as you recommend is very precious.
It is just that I try to find a way to use stronger the rangefinder to get the terrain follow altitude more precise when I’m flying close to the ground.
So, if theire is any Idea I would really apriciate it.


Increasing the RNGFND_GAIN parameter should increase how aggressively it tries to maintain the desired height. If you have a dataflash log I can take a look.

There are some things we can do to improve performance including adding support for multi-beam lidar or allow multiple lidar (pointed in slightly different directions) to be used.

Thank you very much.
I played around with the RNGFND_GAIN alot, even I put it these days at Channel 6 Tuning_41 so I can adjust it quickly with the remote depending on the surface im flying.

I upload two dataflash logs under:

#20 was on a no wind day. The copter was doing a good job, although theire has been the situation where he comes scary close to the ground.
#47 was on a gusty wind day. he was jumping up and down quite bad and came very close to the ground so the second fly I adjust the altitude to 1.5m

I like the Idea of using multiple lidar, maybe the system would recognize then also a little bit earlier a change of the terrain altitude then now, when i have to fly on very complicate terrains very slow with all the disadvantages of slow and close to the ground flying ( downwash & Baro etc)


In the #20 log, the RNGFND_GAIN is set to 1.7 (the default is 0.8) which is too high and results in oscillations. Below is a graph of the desired sonar altitude (CTUN.DSAlt) vs the actual sonar altitude (CTUN.SAlt).

The #47 log actually looks better with the RNGFND_GAIN reduced to 1.27.

One issue though is the Z-axis (i…e vertical) vibration levels are too high. In general vibration levels should be below 30m/s/s. This log shows values that are very often above this so some improved vibration isolation should help.

P.S. for anyone else reading this thread, we are now discussing how to improve terrain following, which is handled outside of the EKF.

1 Like

thanks alot for having a close Look at my Logs.
thats very interesting to look at CTUN.DSAlt and CTUN.SAlt. Everyday you can learn a new Thing. In the End with this Graphs and with putting the RNGFND_GAIN on Channel 6 I can adjust the best settings. Thanks.
The Problem with the Z-axis I do know. Even during the Flight I can see in the Mission Planner that VIBE is turning to red sometimes.
Unfortunately I didnt found a Solution to get that Problem solved.
I´m using the, in my eyes, very handy @proficnc Kore Carrier Board which is absolutely perfect for building Copters. But I didn’t found a System yet to get the Vibrations reduce. At the Moment I put the Board on Rubber VIBRATION DAMPERS M3X15 which seems not to be enough
By the End of January I´m gonna be back in Germany after staying in Australia for a month working.
I´m thinking of building something with O-Rings which is recommended on the
I hope that that gonna help

To help damping vibration Is very effective the use of static and harmonic notch filter

Thank you very much for that Tip. I didn’t know that.
First of all I try to get a better hardware performance and then I’m using this methode

So first I change the hardware. I dicide to take the O-Ring Solution and the Results are enorm:

The performance now in Low Altitude Terrain following got much better:

But if you look at the GPS_Alt you still see the Influence of other Alt Measurements (Baro, GPS, ACC ???) actually I dont know.

If somebody is interested in the whole LOG then you find it here:

Has anyone got an Idea how to reduce the Influence of the Rest of the Measurement and to put more weight on the Sonar Range?

Try increasing RNGFND_GAIN

Vibes are still high. Props balanced ? copter arms stiff enough ?

The RngFnd_Gain I increased until 1.5
As we discussed with @rmackay9 earlier more don’t work. Then the Copter starts jumping up and down.
You think still alot of Vibes? I was so happy to reduce it from 60 to under 10 when until 30 is acceptable.
Then the next step would be to tune the filters…


Has that Parameter something to do with my Problem? Anyone change it? And what is the Result of changing it?