Servers by jDrones

EKF leverages RangeFinder when EK3_RNG_USE_HGT < 0

Hello,

When I tested terrain following, I noticed that EKF keeps using the RangeFinder and eventually targets incorrect altitude even though EK3_RNG_USE_HGT < 0. I am sure whether this is known issue or I misunderstand terrain following of EKF3. Just in case, I would like to explain the issue in here.

  1. As far as I can understand, when EK3_RNG_USE_HGT < 0, EKF will not use the RangeFinder (based on (1) https://github.com/ArduPilot/ardupilot/commit/906731222db17418f5e23472f68f6cdc394cbc9e (2) https://github.com/ArduPilot/ardupilot/issues/12991).
    However, EKF keeps using the RangeFinder even though EK3_RNG_USE_HGT is -1.
    FYI, I used default parameter values except turning on the Rangefinder.

  2. I guess that this issue is come from zero altitude in location.txt for setting starting point as in below.
    Pyramid
    (1) If I assign a proper altitude for the starting point; or (2) If I turn off the RangeFinder, this issue does not occur again.

Log: https://drive.google.com/file/d/155unQ-NkKzcyJw2unMraLT2_dTbyK7b2/view?usp=sharing

Thanks!

Hi @KimHyungSub,

Thanks for your detailed testing and questions.

We have a few wiki pages on terrain following, surface tracking and understanding altitude types but the key point is to be clear which altitude we’re talking about here and why.

If the aim is to do surface tracking or terrain following then the EKF is not involved at all and so no EKF related parameter changes should be made (and you haven’t which is great). The EKF is not responsible for estimating the height above terrain for these use-cases… instead the EKF just does what it always does which is estimate the altitude (above the EKF origin which can be converted to alt-above-home or alt-above-sea-level) using barometer or GPS altitude.

If you look closely into the EKF you may find that it does actually calculate an altitude above terrain but this is only for use with an optical flow sensor.

I’m not sure if this answers your question but hopefully it’s a step towards that and happy to discuss further of course.

1 Like

This discusion may be related to something I have not been able to do in months in the real thing and in SITL: mix in a mission waypoints referenced to Terrain (third parameter 10 (rangefinder)) and Relative (third parameter 3 (barometer)).

This mission in SITL shows it: three W’s in a vertical plane referenced Terrain/Relative/mixed. The mix is:
-High W points Relative (third parameter 3 (barometer)).
-Low W points Terrain (third parameter 10 (rangefinder)).

QGC WPL 110
0	1	0	16	0	0	0	0	33.74113111	-118.374600	44	1
1	0	3	22	0	0	0	0	33.74116111	-118.374600	1.00	1
2	0	3	201	0	0	0	0	33.74111111	-118.375	1.00	1
3	0	10	16	0	0	0	0	33.74111111	-118.37472222	4.00	1
4	0	10	16	0	0	0	0	33.74117111	-118.37472222	1.00	1
5	0	10	16	0	0	0	0	33.74124111	-118.37472222	4.00	1
6	0	10	16	0	0	0	0	33.74117111	-118.37472222	1.00	1
7	0	10	16	0	0	0	0	33.74111111	-118.37472222	4.00	1
8	0	10	16	0	0	0	0	33.74105111	-118.37472222	1.00	1
9	0	10	16	0	0	0	0	33.74099111	-118.37472222	4.00	1
10	0	10	16	0	0	0	0	33.74105111	-118.37472222	1.00	1
11	0	3	16	0	0	0	0	33.74111111	-118.37472222	20.00	1
12	0	3	16	0	0	0	0	33.74117111	-118.37472222	1.00	1
13	0	3	16	0	0	0	0	33.74124111	-118.37472222	20.00	1
14	0	3	16	0	0	0	0	33.74117111	-118.37472222	1.00	1
15	0	3	16	0	0	0	0	33.74111111	-118.37472222	20.00	1
16	0	3	16	0	0	0	0	33.74105111	-118.37472222	1.00	1
17	0	3	16	0	0	0	0	33.74099111	-118.37472222	20.00	1
18	0	3	16	0	0	0	0	33.74105111	-118.37472222	1.00	1
19	0	3	16	0	0	0	0	33.74111111	-118.37472222	20.00	1
20	0	10	16	0	0	0	0	33.74117111	-118.37472222	1.00	1
21	0	3	16	0	0	0	0	33.74124111	-118.37472222	20.00	1
22	0	10	16	0	0	0	0	33.74117111	-118.37472222	1.00	1
23	0	3	16	0	0	0	0	33.74111111	-118.37472222	20.00	1
24	0	10	16	0	0	0	0	33.74105111	-118.37472222	1.00	1
25	0	3	16	0	0	0	0	33.74099111	-118.37472222	20.00	1
26	0	10	16	0	0	0	0	33.74105111	-118.37472222	1.00	1
27	0	10	21	0	0	0	0	33.74119111	-118.374600	0.00	1


The first two W’s are followed normally. For the third, after starting on the high waypoint (third parameter 3), it happens as if it wouldn’t “know” how to go to the following low waypoint (third parameter 10, blue above), “Failsafe terrain data missing” is heard, and the vehicle does an RTL.

This is the video capture:


RTL occurs at t=155s, as appears in the video index.

I don’t know if I am missing something, or this a system limitation I can’t find documented. I have asked about this in the past, resulting the answers the Empty Set ∅.

This happens also in the real thing on all versions I have tried.

This is the SITL parameters file with relevant aparameters:
RNGFND1_MAX_CM 700
TERRAIN_ENABLE 0

Something missing? Is the mixture allowed? Explanation if not?

1 Like

Hi @Webillo,

Thanks for testing 4.1. So I think you’re trying to do terrain following but some of the waypoint altitudes are beyond the range of the rangefinder? If “yes” then I’m afraid this won’t work. For it to do a mission command with a terrain altitude the vehicle must always be flying below the maximum range of the rangefinder.

I think you’ll find that if you switch over to use the terrain database (by setting TERR_ENABLE = 1) and disable the rangefinder that it does work… but of course it won’t be as accurate.

Thanks for your attention.

Possibly this problem was there for ever.

RNGFND1_MAX_CM 700
TERRAIN_ENABLE 0

Looking in detail at the beginning of the third W (mixed 3/10 Relative/Terrain):


Waypoint 19 is at 20m (> RNGFND1_MAX_CM), defined Relative (third parameter 3), and is reached.

Waypoint 20 is at 1m (< RNGFND1_MAX_CM), defined Terrain (third parameter 10), and is not reached ( “ Failsafe terrain data missing ” is heard, and the vehicle does an RTL).

If you go directly to the beginning of the third W, and observe the PIP image (up right) you will see what happens immediately.

Do you mean that it cannot reach a waypoint within rangefinder range (referenced Terrain third parameter 10) from a waypoint out of rangefinder range (referenced Relative third parameter 3) by design? It is hard to accept that.

I simply want to use the rangefinder (which works accurately in the first W (all waypoints Terrain, third parameter 10)) to go from a high point to a low point close to ground accurately.

@Webillo Thank you for the discussion!

I also saw such an error (“Failsafe terrain data missing”) when I mixed relative and AGL altitudes in a mission.

After a vehicle arrives at waypoint 2, it shows “Failsafe terrain data missing” and conduct RTL flight mode because the AGL altitude is higher than 12 meters at waypoint 2.

In my understanding, @rmackay9 says that we need to use the terrain database if any waypoint in the mission exceeds the maximum range of the rangefinder. Please correct me if I am wrong.

I wanted to point out that if I turn on the rangefinder and set TERRAIN_ENABLE = 1, the vehicle keeps targeting incorrect altitude (target altitude is 30 meters in the mission), but the vehicle stays at 52 meters (relative), 85 meters (AGL), and 34 meters (rangefinder).

If this result is the same as you guys’ intention, I’m fine. I left my comment just in case :slight_smile:

@Webillo,

Txs for the extra detail.

After the vehicle passes the waypoint at 20m (rel) and starts towards the waypoints with 1m (terrain) alt it immediately needs to know its altitude above terrain. The reason is that it does not fly a straight line from the rel waypoint to the terr waypoint, instead it flies from it’s current alt above terrain to the waypoint’s alt-above-terrain. The terrain following wiki page’s Warning section has this picture which may explain this.

You hit the nail! Thinking in terms of a “straight line” suggests the solution: before any Terrain waypoint from a Relative one insert a Relative one with same lat/lon coordinates, and height within rangefinder reach:

QGC WPL 110
0	1	0	16	0	0	0	0	33.74113111	-118.3746000	44.0	1
1	0	3	22	0	0	0	0	33.74116111	-118.3746000	1.00	1
2	0	3	201	0	0	0	0	33.74111111	-118.3750000	1.00	1
3	0	10	16	0	0	0	0	33.74111111	-118.37472222	4.00	1
4	0	10	16	0	0	0	0	33.74117111	-118.37472222	1.00	1
5	0	10	16	0	0	0	0	33.74124111	-118.37472222	4.00	1
6	0	10	16	0	0	0	0	33.74117111	-118.37472222	1.00	1
7	0	10	16	0	0	0	0	33.74111111	-118.37472222	4.00	1
8	0	10	16	0	0	0	0	33.74105111	-118.37472222	1.00	1
9	0	10	16	0	0	0	0	33.74099111	-118.37472222	4.00	1
10	0	10	16	0	0	0	0	33.74105111	-118.37472222	1.00	1
11	0	3	16	0	0	0	0	33.74111111	-118.37472222	20.0	1
12	0	3	16	0	0	0	0	33.74117111	-118.37472222	2.00	1
13	0	3	16	0	0	0	0	33.74124111	-118.37472222	20.0	1
14	0	3	16	0	0	0	0	33.74117111	-118.37472222	2.00	1
15	0	3	16	0	0	0	0	33.74111111	-118.37472222	20.0	1
16	0	3	16	0	0	0	0	33.74105111	-118.37472222	2.00	1
17	0	3	16	0	0	0	0	33.74099111	-118.37472222	20.0	1
18	0	3	16	0	0	0	0	33.74105111	-118.37472222	2.00	1
19	0	3	16	0	0	0	0	33.74111111	-118.37472222	20.0	1
20	0	3	16	0	0	0	0	33.74117111	-118.37472222	2.00	1
21	0	10	16	0	0	0	0	33.74117111	-118.37472222	1.00	1
22	0	3	16	0	0	0	0	33.74124111	-118.37472222	20.0	1
23	0	3	16	0	0	0	0	33.74117111	-118.37472222	2.00	1
24	0	10	16	0	0	0	0	33.74117111	-118.37472222	1.00	1
25	0	3	16	0	0	0	0	33.74111111	-118.37472222	20.0	1
26	0	3	16	0	0	0	0	33.74105111	-118.37472222	2.00	1
27	0	10	16	0	0	0	0	33.74105111	-118.37472222	1.00	1
28	0	3	16	0	0	0	0	33.74099111	-118.37472222	20.0	1
29	0	3	16	0	0	0	0	33.74105111	-118.37472222	2.00	1
30	0	10	16	0	0	0	0	33.74105111	-118.37472222	1.00	1
31	0	10	21	0	0	0	0	33.74119111	-118.37460000	0.00	1

(Incidentally, I changed the low W Relative points height to 2m because in practice this seems reliable. YMMV).


I think I tried some similar insertion in the past (possibly not 4.1 yet), without success.

So this is the final video:


Note the transitions.

The PIP (up right) doesn’t show now the RTL trace.

I hope to try the real thing shortly.

1 Like

OK good stuff. Nice video and a good looking path.

This is the real thing, with MP tracking.

Nice, and what also really stands out to me is you’ve got the antenna tracker controlling the camera. I’ve always wanted to do that.

See this. MP works decently well for this; after all, it has all the information after being informed where the tracker is.

You need:

I have pending:

  • Improve pan support.
  • Perhaps try a brushless version.

MP possible improvements:

  • Not sure when servo angles start being output, needed for adjusting offsets before flying. Ideal if just after GPS lock.
  • Pending to test rover in circuit, with tracker above it (done in SILT). I use now a Python script for informing MP that tracker is 4m above rover.

Try it: real fun.

2 Likes

This servo wheel and L piece (attached to the pan servo support), and having substituted the tilt servo support seen in video above


drastically improve tracker stiffness, as can be seen on this video:

There is shown an optional additional spacer around the pan servo wheel.

Next improvement will be a ball bearing on the pan servo wheel:

2 Likes

Here is the gimbal with ball bearing on the pan servo wheel on a rover SITL simulation (tracker up on drivers podium):


Here is the same SITL but on a tlog capture showing the angles (AZtoMAV/EleToMav):

Although the camera moves correctly, these angles are inconsistent:
  • AZtoMAV: goes to zero on the straight beside podium (should vary between 190º and 350º)

  • ELtoMAV: should be clearly under 0º all the time (tracker several meters above car)

1 Like
Servers by jDrones