Terrain height and z-position estimates for rangefinder + optical flow fusion. Solved by Barometer disable.

[ashwinvk94]

The height above ground range used for optical flow fusion is set as _terrain_vpos - z_position. But when I am using a rangefinder(EKF2_HGT_MODE is set rangefinder), the z position is directly measuring the height above the ground. In this situation, shouldn't the heightAboveGndEst be equal to _state.pos(2) or z_position?

[bresch]

Even in range finder primary height mode, the inertial data is used and the vertical GPS velocity is fused (if available). This means that even if the terrain height changes, the altitude of the EKF (state.pos(2)) will be stabilized by the other sensors and a relative terrain estimate can be obtained.

[ashwinvk94]

I had set the EKF2_HGT_MODE to range finder. But in a previous comment, you had mentioned that "terrain estimator only works properly if the range finder is not used as the primary height source".

So I set EKF2_HGT_MODE to barometer. Now, the range estimates(_terrain_vpos and state.pos(2)) are good. But the problem is that the erroneous fluctuations in the barometer reading causes the height estimate to change according to it.

In the the below flight, the drone took off, flew at about 1m in height and landed. In the graph you can see that the distance sensor reading is always around 1 m, but bad baro_vpos values cause the vehicle_local_position/z vary according to it.

Please not that estimator_optical_flow/vel_ne.01 = _terrain_vpos

zipped_log_file

Is there any way I can use set the EKF2_HGT_MODE to range finder and still use the optical flow fusion code? This code uses _terrain_vpos from the terrain estimator.

[ashwinvk94]

This issue was solved by the following steps:

1. EKF2_HGT_MODE was set to rangefinder
2. barometer aiding was removed by commenting out the following lines 1 and 2 in control.cpp. And replacing the line that initializes the baro as the height feedback with the line that initializes rangefinder as the height feedback initialization(setControlRangeHeight()). This ensured that the rangefinder would solely be used to estimate the the altitude of the EKF (state.pos(2))
3. heightAboveGndEst was calculated solely using the altitude of the EKF (state.pos(2)) and does not use the terrain estimator. This line,float heightAboveGndEst = _terrain_vpos - _state.pos(2); was replaced with float heightAboveGndEst = - _state.pos(2);
   This resulted in successful/ stable flight in position mode. Note that in this solution the drone solely relies on the rangefinder for height estimation(and optical flow vel estimation subsequently). But it works for my specific use case.