Hover thrust estimator #13981

bresch · 2020-01-19T16:38:29Z

Describe problem solved by this pull request
The hover thrust is an important parameter of the vehicle that is used during flight for altitude control and to detect landing. It needs to be set manually by the user (MPC_THR_HOVER), and can trigger a crash if wrongly set.

Furthermore, this parameter is constant, but In reality, the real hover thrust is not:

The battery voltage decreases during the flight -> requires more throttle if RPM isn't controlled by the ESC
The mass can change (e.g.: cargo drones, crop sprayers, ...)

Finally, some vehicles with many possible payloads can have a different hover thrust at almost every takeoff.

Describe your solution
Given the current thrust and the measured vertical acceleration, it is possible to observe the hover thrust. This is done in this PR using a simple single-state EKF.
The model is described and simulated here.

Describe possible alternatives
Some logic that tries to detect a hover to set the hover thrust as the current thrust.
A higher order model

Test data / coverage
Current state: SITL tests, the estimator converges to the correct value. When changing the mass during hover, the estimate follows quickly.

Additional context
Auterion/Flight_Control_Prototyping_Scripts#3

TODO:

unit tests!
log the state and main important values (variance, innov, test ratio, ...)
use the estimated hover thrust in PositionControl and the LandDetector edit: this will be done later
check dynamic response (up/down motions)
use unfiltered acceleration (edit: I'm using the acceleration in local frame from EKF2 and it seems to work quite well)
~~add thrust to acceleration dynamic model (first order filter should be enough)~~ edit: not required, the performance is good enough
make sure it doesn't update during the ground contact phase

fixes #12439

jkflying

This looks awesome.

An idea, how about we already use the value of the estimated hover thrust if the parameter is set to 0? Then existing users won't be affected, but with a single parameter change can get the new behavior. We can set the default to 0 once we are happy with the performance.

Also, I'm very interested to see how the convergence speed is on initialization, compared to noise rejection over longer periods. There might be value in using vertical velocity estimate plus a drag model to improve accuracy - but I'm not sure if that will be necessary, since this is already a huge improvement over a single hardcoded value.

jkflying · 2020-01-19T17:20:12Z

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -603,6 +607,11 @@ MulticopterPositionControl::Run()
 				_control.resetIntegral();
 				// reactivate the task which will reset the setpoint to current state
 				_flight_tasks.reActivate();
+				_hover_thrust_estimator.reset();
+
+			} else if (_takeoff.getTakeoffState() >= TakeoffState::flight) {


Do we need to handle landing as well to make sure it doesn't get corrupted? Or will this work for the landing case too?

Good point, i would need to check that

Why is this in the else case? Would you want to run it if PX4_ISFINITE(setpoint.thrust[2])?

@bresch did you check this?

@mhkabir Yes, due to the high inconsistency at touchdown, the measurements are rejected until the recovery kicks-in and we should be able to detect landing in the mean time (using that inconsistency as a ground contact check).
About the "Why is this in the else case? Would you want to run it if PX4_ISFINITE(setpoint.thrust[2])?", I placed it in that else because it gets reset when on ground in the if above, so we don't need to do a double check. It might not be ideal but I can still change that when I use the output of the filter for feedforward and land detection.

src/lib/hover_thrust_estimator/ZeroOrderHoverThrustEkf.cpp

RomanBapst · 2020-01-20T07:37:45Z

src/lib/hover_thrust_estimator/hover_thrust_estimator_params.c

+/**
+ * Hover thrust process noise
+ *
+ * Bla


@bresch Bla? :-D

Isn't that clear enough? xD

src/lib/hover_thrust_estimator/ZeroOrderHoverThrustEkf.cpp

RomanBapst · 2020-01-20T08:34:01Z

@bresch Btw, I really like to use these lecture note as reference.
https://idsc.ethz.ch/education/lectures/recursive-estimation.html

I think it provides a really good summary in case you don't always want to read 1000 pages in the Kalman filter book.

src/lib/hover_thrust_estimator/ZeroOrderHoverThrustEkf.hpp

kamilritz

Great Work, I like the separation of the HoverThrustEstimator and ZeroOrderHoverThrustEstimator.
Are the local_pos_sp.thrust and _states.acceleration in local/world coordinates?

src/lib/hover_thrust_estimator/CMakeLists.txt

src/lib/hover_thrust_estimator/HoverThrustEstimator.hpp

src/lib/hover_thrust_estimator/ZeroOrderHoverThrustEkf.cpp

src/lib/hover_thrust_estimator/ZeroOrderHoverThrustEkf.hpp

dagar · 2020-01-21T16:44:43Z

If you rebase there's a big chunk of flash available on fmu-v2 now.

bresch · 2020-01-23T13:07:08Z

SITL jMavSim test where the weight of the vehicle is changed during flight from 0.4 to 0.8kg.
Initially the measurement is rejected (large test ratio) but then the measurement variance blows-up and the state slowly starts to converge towards the new true value. This way, no reset logic is required and the state is continuous.

bresch · 2020-01-23T13:35:07Z

@PX4/testflights Can you give that PR a quick try please? This is just for logging, don't expect anything different.

Junkim3DR · 2020-01-23T21:23:41Z

Tested on NXP FMUK66 v3

Modes Tested
Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint then trigger RTL.

Notes
No issues noted, good flight in general.

Log

https://review.px4.io/plot_app?log=b70ebf6f-18ab-4475-8541-cc6d0c76bbee

jorge789 · 2020-01-23T21:29:11Z

Tested on PixRacer V4

Modes Tested
Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint then trigger RTL.

Notes
No issues noted, good flight in general.

Log
https://review.px4.io/plot_app?log=64209608-53fe-4c6d-94bd-b92bf7109188

Tested on CUAV nano V5

Modes Tested
Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint then trigger RTL.

Notes
No issues noted, good flight in general.

Log
https://review.px4.io/plot_app?log=debc4edf-477d-4fde-a23c-661bebf8371c

dannyfpv · 2020-01-23T22:37:06Z

Tested on Pixhawk 4 V5 f-450
Modes Tested
Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Takeoff in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceeds to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint then trigger RTL.

Notes
No issues noted, good flight in general.

Log
https://review.px4.io/plot_app?log=95d42940-52c1-409b-a458-3505ef91bd32

bresch · 2020-01-24T06:56:13Z

Thanks @Junkim3DR @dannyfpv and @jorge789 !

MaEtUgR

Nice architecture with the interface class!

src/lib/hover_thrust_estimator/hover_thrust_estimator.hpp

MaEtUgR · 2020-01-24T11:01:53Z

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -603,6 +607,11 @@ MulticopterPositionControl::Run()
 				_control.resetIntegral();
 				// reactivate the task which will reset the setpoint to current state
 				_flight_tasks.reActivate();
+				_hover_thrust_estimator.reset();
+
+			} else if (_takeoff.getTakeoffState() >= TakeoffState::flight) {


Why is this in the else case? Would you want to run it if PX4_ISFINITE(setpoint.thrust[2])?

src/lib/hover_thrust_estimator/hover_thrust_estimator.hpp

bresch · 2020-01-28T16:51:32Z

@PX4/testflights could you make a few more flights please?

codecov · 2020-01-29T12:56:15Z

Codecov Report

Merging #13981 into master will decrease coverage by 6.13%.
The diff coverage is 98.16%.

@@            Coverage Diff             @@
##           master   #13981      +/-   ##
==========================================
- Coverage   42.32%   36.19%   -6.14%     
==========================================
  Files         608      552      -56     
  Lines       51322    47108    -4214     
==========================================
- Hits        21723    17051    -4672     
- Misses      29599    30057     +458

Flag	Coverage Δ
#mavsdk	`36.19% <98.16%> (+0.08%)`	⬆️
#python	`?`
#sitl_mission_FW	`?`
#sitl_mission_MC_box	`?`
#sitl_mission_MC_offboard_att	`?`
#sitl_mission_MC_offboard_pos	`?`
#sitl_mission_Rover	`?`
#sitl_mission_VTOL_standard	`?`
#sitl_mission_VTOL_tailsitter	`?`
#sitl_python_FW	`?`
#sitl_python_MC_box	`?`
#sitl_python_MC_offboard_att	`?`
#sitl_python_MC_offboard_pos	`?`
#sitl_python_Rover	`?`
#sitl_python_VTOL_standard	`?`
#sitl_python_VTOL_tailsitter	`?`
#unittest	`?`

Impacted Files	Coverage Δ
src/modules/mc_pos_control/mc_pos_control_main.cpp	`60.59% <100%> (-3.96%)`	⬇️
...r_thrust_estimator/zero_order_hover_thrust_ekf.hpp	`100% <100%> (ø)`
src/modules/logger/logged_topics.cpp	`70.9% <100%> (+0.13%)`	⬆️
...r_thrust_estimator/zero_order_hover_thrust_ekf.cpp	`100% <100%> (ø)`
.../hover_thrust_estimator/hover_thrust_estimator.hpp	`87.5% <87.5%> (ø)`
.../hover_thrust_estimator/hover_thrust_estimator.cpp	`96.29% <96.29%> (ø)`
...modules/rover_pos_control/RoverPositionControl.hpp	`0% <0%> (-100%)`	⬇️
src/modules/land_detector/RoverLandDetector.cpp	`0% <0%> (-100%)`	⬇️
src/modules/vtol_att_control/tailsitter.cpp	`0% <0%> (-94.45%)`	⬇️
...fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp	`6.09% <0%> (-93.91%)`	⬇️
... and 118 more

Junkim3DR · 2020-01-29T17:31:13Z

Tested on NXP FMUK66 v3

Modes Tested

Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint then trigger RTL.

Notes
No issues noted, good flight in general.

Log

https://review.px4.io/plot_app?log=3c0e264d-6260-4305-bbd8-b08178c4adf8

dagar · 2020-02-05T15:35:38Z

msg/tools/uorb_rtps_message_ids.yaml

@@ -352,4 +352,7 @@ rtps:
  - msg: estimator_innovation_test_ratios
    id: 171
    alias: estimator_innovations
+  - msg: hover_thrust_estimator
+    id: 172
+    alias: hover_thrust_estimator


This needs to be moved to the earlier section (128).

mhkabir · 2020-02-05T15:42:23Z

msg/CMakeLists.txt

@@ -68,6 +68,7 @@ set(msg_files
 	gps_dump.msg
 	gps_inject_data.msg
 	home_position.msg
+	hover_thrust_estimator.msg


It would be better if this was hover_thrust_estimate

makes sense

bresch · 2020-02-06T08:34:01Z

TODO:

address Kabir's and Daniel's comments
cleanup commits

bresch · 2020-02-06T22:06:01Z

SITL test with a lot of changes in the hover thrust during the mission. This shows the robustness of the filter and its capability to converge towards the new correct value.

jkflying

LGTM, tests all passing

with measurement noise auto-tuning The purpose of this estimator is to improve land detection and vertical velocity feedforward Recovery strategy: This is required when the setpoint suddenly changes in air or that the EKF is diverging. A lowpassed test ratio is used as a trigger for the recovery logic Also, a lowpassed residual is used to estimate the steady-state value and remove it when estimating the accel noise to avoid increasing the accel noise when the redisual is caused by an offset.

With noisy accel and thrust in hover, climb and descent conditions.

This is done to test the recovery function of the estimator in case of divergence or sudden extreme hover thrust change. Also specify seed of random generator

bkueng · 2020-02-07T09:52:54Z

src/lib/hover_thrust_estimator/zero_order_hover_thrust_ekf_test.cpp

@@ -47,32 +47,37 @@ using namespace matrix;
 class ZeroOrderHoverThrustEkfTest : public ::testing::Test
 {
 public:
+	ZeroOrderHoverThrustEkfTest()
+	{
+		_random_generator.seed(42);


Just FYI (and lessons learned from previous hair-pulling), these still won't be the same across platforms or std libraries... 😢

@jkflying hmm, okay, should I run once the Gaussian noise generator and save the output in a file to be sure that we always use the same data?

bresch · 2020-02-07T11:00:26Z

Finally merged, thanks everyone for the help! :)

VTOLDavid · 2020-06-09T13:47:31Z

What would happen in a VTOL in hover with wind? The hover thurst can be really low.

bresch · 2020-06-11T07:49:33Z

@VTOLDavid If it's a constant wind and the hover thrust is constantly lower, the estimator will converge to this new value. If it's just wind gusts, the estimator will reject that and keep the same value.
It's also a similar effect during VTOL transitions the hover thrust changes but it isn't an issue, the estimator has a recovery strategy that allows it to converge to a new estimate, even if far from the initial one.
You can do some SITL tests or check if you can find public VTOL logs to see how it behaves (check the hover_thrust_estimate topic).

VTOLDavid · 2020-06-15T08:59:14Z

Thank you for the explanation.

DronecodeBot · 2024-06-18T00:31:53Z

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/multicopter-with-1kg-payload-px4/39275/3

bresch added Admin: Enhancement (improvement) 💡 Multirotor 🛸 labels Jan 19, 2020

bresch requested review from priseborough, RomanBapst and kamilritz January 19, 2020 16:38

bresch self-assigned this Jan 19, 2020

jkflying reviewed Jan 19, 2020

View reviewed changes

RomanBapst reviewed Jan 20, 2020

View reviewed changes

src/lib/hover_thrust_estimator/ZeroOrderHoverThrustEkf.cpp Outdated Show resolved Hide resolved

RomanBapst reviewed Jan 20, 2020

View reviewed changes

src/lib/hover_thrust_estimator/ZeroOrderHoverThrustEkf.cpp Outdated Show resolved Hide resolved

bresch commented Jan 20, 2020

View reviewed changes

src/lib/hover_thrust_estimator/ZeroOrderHoverThrustEkf.hpp Outdated Show resolved Hide resolved

bresch commented Jan 20, 2020

View reviewed changes

src/lib/hover_thrust_estimator/ZeroOrderHoverThrustEkf.hpp Outdated Show resolved Hide resolved

kamilritz reviewed Jan 20, 2020

View reviewed changes

bresch force-pushed the pr-hover-ekf branch 2 times, most recently from e37ce88 to 5394248 Compare January 23, 2020 13:01

bresch requested a review from a team January 23, 2020 14:18

MaEtUgR reviewed Jan 24, 2020

View reviewed changes

bkueng reviewed Jan 27, 2020

View reviewed changes

src/lib/hover_thrust_estimator/hover_thrust_estimator.hpp Outdated Show resolved Hide resolved

bresch force-pushed the pr-hover-ekf branch from 27faceb to 9848300 Compare January 29, 2020 10:31

bresch force-pushed the pr-hover-ekf branch from 26f993c to 25e8f29 Compare February 1, 2020 17:54

weekly-digest bot mentioned this pull request Feb 2, 2020

Weekly Digest (26 January, 2020 - 2 February, 2020) #14084

Closed

dagar reviewed Feb 5, 2020

View reviewed changes

mhkabir reviewed Feb 5, 2020

View reviewed changes

bresch force-pushed the pr-hover-ekf branch from 85ffebf to a151995 Compare February 6, 2020 22:03

dagar previously approved these changes Feb 7, 2020

View reviewed changes

bresch dismissed dagar’s stale review via 1efbc62 February 7, 2020 09:15

bresch force-pushed the pr-hover-ekf branch from ccdff17 to 1efbc62 Compare February 7, 2020 09:15

jkflying previously approved these changes Feb 7, 2020

View reviewed changes

bresch added 4 commits February 7, 2020 10:50

HoverThrustEstimator: Add convergence tests

c01d074

With noisy accel and thrust in hover, climb and descent conditions.

ZeroOrderHoverThrustEstimatorTest: add hover thrust jump test case

1457999

This is done to test the recovery function of the estimator in case of divergence or sudden extreme hover thrust change. Also specify seed of random generator

uorb_rtps_message_ids: add hover_thrust_estimate msg

b3f61ed

bresch dismissed jkflying’s stale review via b3f61ed February 7, 2020 09:51

bresch force-pushed the pr-hover-ekf branch from 1efbc62 to b3f61ed Compare February 7, 2020 09:51

bkueng approved these changes Feb 7, 2020

View reviewed changes

bresch merged commit ca0b570 into master Feb 7, 2020

bresch deleted the pr-hover-ekf branch February 7, 2020 10:52

weekly-digest bot mentioned this pull request Feb 9, 2020

Weekly Digest (2 February, 2020 - 9 February, 2020) #14124

Closed

RomanBapst mentioned this pull request Feb 17, 2020

Thrust value oscillating (repeatedly dropping to 0) #13507

Closed

MaEtUgR mentioned this pull request Feb 24, 2020

[WIP] adaptive hover-throttle for altitude control #12534

Closed

dagar mentioned this pull request Feb 27, 2020

move hover_thrust_estimator to new module (mc_hover_thrust_estimator) #14182

Merged

bresch mentioned this pull request Mar 17, 2020

v1.11.0 Release Notes #14402

Closed

Hover thrust estimator #13981

Hover thrust estimator #13981

Conversation

bresch commented Jan 19, 2020 • edited Loading

jkflying left a 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 Jan 20, 2020

kamilritz left a comment

Choose a reason for hiding this comment

dagar commented Jan 21, 2020

bresch commented Jan 23, 2020

bresch commented Jan 23, 2020

Junkim3DR commented Jan 23, 2020

Tested on NXP FMUK66 v3

jorge789 commented Jan 23, 2020

Tested on PixRacer V4

Tested on CUAV nano V5

dannyfpv commented Jan 23, 2020 • edited by Tony3dr Loading

bresch commented Jan 24, 2020

MaEtUgR left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

bresch commented Jan 28, 2020

codecov bot commented Jan 29, 2020 • edited Loading

Codecov Report

Junkim3DR commented Jan 29, 2020

Tested on NXP FMUK66 v3

Choose a reason for hiding this comment

mhkabir Feb 5, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

bresch commented Feb 6, 2020 • edited Loading

bresch commented Feb 6, 2020

jkflying left a 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

bresch commented Feb 7, 2020

VTOLDavid commented Jun 9, 2020

bresch commented Jun 11, 2020

VTOLDavid commented Jun 15, 2020

DronecodeBot commented Jun 18, 2024

bresch commented Jan 19, 2020 •

edited

Loading

dannyfpv commented Jan 23, 2020 •

edited by Tony3dr

Loading

MaEtUgR left a comment •

edited

Loading

codecov bot commented Jan 29, 2020 •

edited

Loading

mhkabir Feb 5, 2020 •

edited

Loading

bresch commented Feb 6, 2020 •

edited

Loading