Implemented use of optical flow for terrain estimation #597

RomanBapst · 2019-04-14T11:38:48Z

This PR allows optical flow measurements to be used for terrain estimation. I see the following two cases when this could be useful:

terrain estimation for fixed wing flight and auto land (potentially using the flow sensor at higher altitude and a lidar when close to the ground)
@priseborough already did this in another project and I believe he had good success with the pmw3901 optical flow sensor.
enhancement of the detection logic for ground effect (see Pr ground effect zone PX4-Autopilot#11592 )

Flow measurements will only be fused in the terrain estimator if no range data is being fused and if the primary EKF is not fusing flow.
So far this has only been tested in SITL using iris_opt_flow model. Two logs are being provided, one where the terrain estimate is initialized to 50m on the ground in order to see if the filter recovers.
The second log shows a normal flight in position control over a flat surface. As the vehicle starts to move the terrain height becomes observable and the estimated terrain variance decreases. When the vehicle holds position the terrain variance increases again.

Terrain initialized to 50m

Normal flight over flat ground

Logs:
Terrain initialized to 50m:
https://logs.px4.io/plot_app?log=e47c69ed-125c-4a82-a1df-dc0553904b63

Normal flight:
https://logs.px4.io/plot_app?log=a3864090-d60c-42af-8fc8-f8465ded7790

How to test:
Currently this feature can only be tested if optical flow in the main filter is deactivated and if no range data is available. Therefore, the following two steps are required for testing:

Set EKF2_AID_MASK to only use GPS
Comment out this line to avoid the system publishing range finder data.

Required action items:

test on real vehicle (multicopter and fixed wing)
- make sure we do not break existing optical flow & terrain estimation via lidar
- potentially adjust innovation gate size, optical flow measurement noise for robustness

RomanBapst · 2019-04-15T06:03:04Z

EKF/terrain_estimator.cpp

+	}
+
+	// Calculate observation matrix for flow around the vehicle y axis
+	float Hy = -vel_body(0) * t0 /((_terrain_vpos - _state.pos(2)) * (_terrain_vpos - _state.pos(2)));


Guard against division by zero

RomanBapst · 2019-04-18T07:17:19Z

Results from test flight on a fixed wing (Quanum observer, cruise speed 15 m/s), this was a flight over more or less flat terrain.

This is a zoom-in into the region when the vehicle landed. Noticed that at the time of touch-down the estimated distance to bottom is about 26cm (indicated by the spike in the accelerometer).

RomanBapst · 2019-04-18T07:19:38Z

Results from a flight over a hill with altitudes above 100m.

RomanBapst · 2019-04-18T07:38:01Z

Replay log for flat terrain:
https://logs.px4.io/plot_app?log=925aeea4-6679-4452-904e-1a1144e45bc2

Replay log for flight over hill:
https://logs.px4.io/plot_app?log=869142b1-7eeb-41d5-828a-3f7fefd848fe

NOTE
The optical flow orientation on these flight was not correct and need to be adjusted if you want to replay the logs. You can add the following code here

float helper = optflow_sample_new.flowRadXY(0);
optflow_sample_new.flowRadXY(0) = -optflow_sample_new.flowRadXY(1);
optflow_sample_new.flowRadXY(1) = helper;

RomanBapst · 2019-04-27T16:41:15Z

This is a replayed log file of when the plane was flying over a hill:
https://logs.px4.io/plot_app?log=7b365163-79fc-46e7-8615-1b46d08db4a6

priseborough · 2019-05-01T05:18:40Z

EKF/terrain_estimator.cpp

@@ -55,7 +55,10 @@ bool Ekf::initHagl()
 		_terrain_var = sq(_params.range_noise);
 		// success
 		return true;
-
+	} else if (_flow_for_terrain_data_ready) {
+		_terrain_vpos = _state.pos(2);


This is a guess unless we are on the ground. Might be better to set to height of origin as that is where terrain is most likely to be after takeoff.

If on ground, using current position is better.

priseborough · 2019-05-01T05:37:35Z

EKF/terrain_estimator.cpp

+	// rotate into body frame
+	Vector3f vel_body = earth_to_body * vel_rel_earth;
+
+	float t0 = q0*q0 - q1*q1 - q2*q2 + q3*q3;


Which derivation did you use to obtain this? If you have a Matlab file, add it here and provide a link to it in the documentation: https://github.com/PX4/ecl/tree/master/EKF/matlab/scripts/Terrain%20Estimator

@priseborough I added a python folder which has the script for the derivation.

priseborough · 2019-05-01T05:39:10Z

EKF/terrain_estimator.cpp

+	float Hx = vel_body(1) * t0 /(pred_hagl * pred_hagl);
+
+	// Cacluate innovation variance
+	_flow_innov_var[0] = Hx * Hx * _terrain_var + R_LOS;


Add protection against Hx * Hx * _terrain_var being less then zero.

There are many places in code that touch _terrain_var and it is not forced to the non-negative immediately prior.

priseborough · 2019-05-01T05:39:34Z

EKF/terrain_estimator.cpp

+	float Hy = -vel_body(0) * t0 /(pred_hagl * pred_hagl);
+
+	// Calculuate innovation variance
+	_flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS;


Add protection against Hy * Hy * _terrain_var being less then zero.

_terrain_var being protected against being negative above so should be OK.

Signed-off-by: Roman <[email protected]>

…rain height estimation) Signed-off-by: Roman <[email protected]>

RomanBapst · 2019-05-12T17:01:39Z

@priseborough I addressed your comments.

RomanBapst requested a review from priseborough April 14, 2019 11:47

RomanBapst commented Apr 15, 2019

View reviewed changes

dagar added ekf enhancement labels Apr 16, 2019

RomanBapst force-pushed the terrain_flow branch from c8fe96d to b9f909b Compare April 27, 2019 16:23

priseborough reviewed May 1, 2019

View reviewed changes

RomanBapst added 4 commits May 12, 2019 18:33

implemented use of optical flow for terrain estimation

e7c5017

Signed-off-by: Roman <[email protected]>

terrain estimator: constrain terrain state properly

0aae168

Signed-off-by: Roman <[email protected]>

addressed review comments

cec264f

added python script for terrain flow derivation (optical flow for ter…

67a4d96

…rain height estimation) Signed-off-by: Roman <[email protected]>

RomanBapst force-pushed the terrain_flow branch from b9f909b to 67a4d96 Compare May 12, 2019 17:00

priseborough approved these changes May 14, 2019

View reviewed changes

priseborough merged commit 770cd3c into PX4:master May 15, 2019

bkueng mentioned this pull request May 15, 2019

ekf2: reduce GPS initialization time for SITL from 10s to 0.5s PX4/PX4-Autopilot#12021

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Implemented use of optical flow for terrain estimation #597

Implemented use of optical flow for terrain estimation #597

RomanBapst commented Apr 14, 2019 •

edited

Loading

RomanBapst Apr 15, 2019

RomanBapst commented Apr 18, 2019

RomanBapst commented Apr 18, 2019

RomanBapst commented Apr 18, 2019

RomanBapst commented Apr 27, 2019

priseborough May 1, 2019

priseborough May 1, 2019

RomanBapst May 12, 2019

priseborough May 1, 2019

RomanBapst May 12, 2019

priseborough May 1, 2019

priseborough May 1, 2019

RomanBapst May 12, 2019

priseborough May 1, 2019

priseborough May 1, 2019

RomanBapst May 12, 2019

RomanBapst commented May 12, 2019

Implemented use of optical flow for terrain estimation #597

Implemented use of optical flow for terrain estimation #597

Conversation

RomanBapst commented Apr 14, 2019 • edited Loading

Choose a reason for hiding this comment

RomanBapst commented Apr 18, 2019

RomanBapst commented Apr 18, 2019

RomanBapst commented Apr 18, 2019

RomanBapst commented Apr 27, 2019

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

RomanBapst commented May 12, 2019

RomanBapst commented Apr 14, 2019 •

edited

Loading