ekf2_main - Add optical flow innovation pre-flight check #13036

bresch · 2019-09-27T14:32:07Z

Currently, only the innovations from the vel_pos_innov array are checked. Since the optical flow innovations are in flow_innov, they are never checked and when flying optical flow only, the GPS velocities innovations are still checked.
To fix this inconsistency, this PR adds optical flow innovation checks and generally, the checks are only performed if a the sensor is fused.
I also refactored a bit the whole pre-flight check part of this module to make it more scalable and readable. Adding a new check (e.g.: adding a new check should be easy).

The check is also the same for all the innovations:
fail if (low-pass(innov) > test or innov > 2x test)

mhkabir · 2019-09-27T15:22:19Z

src/modules/ekf2/ekf2_main.cpp

+{
+	bool has_failed = false;
+
+	bool doing_ne_aiding = control_status.flags.gps ||  control_status.flags.ev_pos;


Suggested change

bool doing_ne_aiding = control_status.flags.gps || control_status.flags.ev_pos;

bool doing_hpos_aiding = control_status.flags.gps || control_status.flags.ev_pos;

@mhkabir Since you want to rename that hpos because EV fusion doesn't mean NE navigation, it means that you don't actually really care of heading. Should we then use the test limit in the else part of the selection?

bkueng

Good cleanup.

src/modules/ekf2/Utility/InnovationLpf.cpp

src/modules/ekf2/ekf2_main.cpp

src/modules/ekf2/Utility/InnovationLpf.cpp

bresch · 2019-10-01T12:41:50Z

@bkueng Thanks for the review, I did the corrections, can you review again please?

bkueng · 2019-10-02T08:38:10Z

Looks good.

priseborough · 2019-10-03T08:55:16Z

How has this been tested? Can you supply a log?

priseborough · 2019-10-03T09:16:14Z

it could be tested using replay with post filtered innovations printed to file at a reduced rate, eg 1Hz and we could then compare the files before and after to check that the filtering is still working the same.

CarlOlsson · 2019-10-03T12:14:38Z

Nice! Would be awesome if we could add a bitfield for which check that is failing since it can be a bit annoying to debug since we don't log the low pass filtered innovations

bresch · 2019-10-16T09:08:06Z

@CarlOlsson I changed the "master check" for a bitfield

CarlOlsson · 2019-10-16T15:46:21Z

@CarlOlsson I changed the "master check" for a bitfield

Awesome!

msg/estimator_status.msg

…ass filtering

spike limit during the update call to avoid storing it here

- set spike_lim constants as static constexpr, set innovation - set checker helper functions as static - rename the mix of heading and yaw as heading to avoid confusion

…sed for logging

pre-flight checks. Add simple unit testing Use bitmask instead of general flag to have more granularity

…sending booleans in the update function This makes it more scalable as more checks will be added

Rename "down" to "vert"

msg/estimator_status.msg

jorge789 · 2019-10-21T21:27:22Z

Tested on PixRacer V4:

Modes Tested
Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL: 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 activate RTL and see landing behavior.

Notes:
No issues noted good flight in general.

Log:
https://review.px4.io/plot_app?log=41494978-b2c3-4db3-825d-f932e4031101

https://review.px4.io/plot_app?log=bce2449c-7829-4545-a932-53449c2a84f4

https://review.px4.io/plot_app?log=ffbd784e-7202-4edd-98fe-636e666ee03e

Master comparison:
https://review.px4.io/plot_app?log=2de4cc7b-b4b8-4b84-8a0e-0378f86dafcc

https://review.px4.io/plot_app?log=307eac73-9648-4526-8b35-5b52a1ebb4ae

https://review.px4.io/plot_app?log=c06841d2-0474-43f6-979f-9cc3796b9705

jorge789 · 2019-10-21T22:13:11Z

Tested on Pixhawk 4 V5:
Modes Tested:
Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL: 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 activate RTL and see landing behavior.

Notes:
No issues noted good flight in general.

Log:
https://review.px4.io/plot_app?log=22706bcb-7b91-43a4-8a04-caf20ccb4b7f&fbclid=IwAR0i9VIsraI2eXvt6FxgFEF_VcoUhi4gJPCJl1PD_QUd8c8g6vc0D6rVaqk

Tested on Pixhawk pro v4:
Modes Tested

Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL: 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 activate RTL and see landing behavior.

Notes:
No issues noted good flight in general.

Log:
https://review.px4.io/plot_app?log=73d259d0-4fae-4812-bb5b-31410a5a4207&fbclid=IwAR1J1apCKJH3pmBfEHa24zer0ce6HjyM2Ug_A1PYMNd-2RAVQ47KxeeYcFc

bresch · 2019-10-22T11:49:32Z

Thanks @jorge789 !

bresch · 2019-10-22T13:00:51Z

@dagar Can you approve and merge? I also made a few tests on a bench setup to see the error messages.

* ekf2: Add FirstOrderLpf and InnovationLpf classes for innovation lowpass filtering * ekf2: use InnovLpf filter class in preflight checks * ekf2: move selection of yaw test limit for pre-flight check in function * ekf2: Move pre-flight checks into separate function * ekf2: use static constexpr insetead of inline for sq (square) function * ekf2: Split pre-flight checks in separate functions Also use the same check for all the innovations: innov_lpf < test and innov < 2xtest * ekf2: Add optical flow pre-flight check * ekf2: Combine FirstOrderLpf and InnovationLpf in single class * ekf2: check vel_pos_innov when ev_pos is active as well * ekf2: transform InnovationLpf into a header only library and pass the spike limit during the update call to avoid storing it here * ekf2: Static and const cleanup - set spike_lim constants as static constexpr, set innovation - set checker helper functions as static - rename the mix of heading and yaw as heading to avoid confusion * ekf2: use ternary operator in selectHeadingTestLimit instead of if-else * ekf2: store intermediate redults in const bool flags. Those will be used for logging * ekf2: set variable const whenever possible * ekf2: create PreFlightChecker class that handle all the innovation pre-flight checks. Add simple unit testing Use bitmask instead of general flag to have more granularity * PreFlightChecker: use setter for the innovations to check instead of sending booleans in the update function This makes it more scalable as more checks will be added * ekf: Use booleans instead of bitmask for ekf preflt checks Rename "down" to "vert"

bresch added Admin: Enhancement (improvement) 💡 EKF2 Admin: Cleanup (housekeeping) 🧹 labels Sep 27, 2019

bresch requested review from bkueng, jkflying and priseborough September 27, 2019 14:32

bresch self-assigned this Sep 27, 2019

mhkabir reviewed Sep 27, 2019

View reviewed changes

weekly-digest bot mentioned this pull request Sep 29, 2019

Weekly Digest (22 September, 2019 - 29 September, 2019) #13050

Closed

bkueng reviewed Sep 30, 2019

View reviewed changes

bresch force-pushed the dev-ekf-prearm branch 2 times, most recently from 4ddbac3 to acdd046 Compare October 1, 2019 12:28

bresch force-pushed the dev-ekf-prearm branch from acdd046 to 25390ac Compare October 1, 2019 12:43

weekly-digest bot mentioned this pull request Oct 6, 2019

Weekly Digest (29 September, 2019 - 6 October, 2019) #13096

Closed

bresch force-pushed the dev-ekf-prearm branch 2 times, most recently from 71efbea to d029973 Compare October 16, 2019 09:02

bresch marked this pull request as ready for review October 16, 2019 09:07

dagar reviewed Oct 16, 2019

View reviewed changes

msg/estimator_status.msg Outdated Show resolved Hide resolved

weekly-digest bot mentioned this pull request Oct 20, 2019

Weekly Digest (13 October, 2019 - 20 October, 2019) #13236

Closed

bresch added 4 commits October 21, 2019 12:17

ekf2: Add FirstOrderLpf and InnovationLpf classes for innovation lowp…

4f3974a

…ass filtering

ekf2: use InnovLpf filter class in preflight checks

555bb23

ekf2: move selection of yaw test limit for pre-flight check in function

33cb18c

ekf2: Move pre-flight checks into separate function

5fd6349

bresch added 11 commits October 21, 2019 12:17

ekf2: Add optical flow pre-flight check

480b990

ekf2: Combine FirstOrderLpf and InnovationLpf in single class

5bbaf6b

ekf2: check vel_pos_innov when ev_pos is active as well

6f5b75e

ekf2: transform InnovationLpf into a header only library and pass the

a0bbc5c

spike limit during the update call to avoid storing it here

ekf2: Static and const cleanup

4bef660

- set spike_lim constants as static constexpr, set innovation - set checker helper functions as static - rename the mix of heading and yaw as heading to avoid confusion

ekf2: use ternary operator in selectHeadingTestLimit instead of if-else

e2c4683

ekf2: store intermediate redults in const bool flags. Those will be u…

ebce352

…sed for logging

ekf2: set variable const whenever possible

f82d897

ekf2: create PreFlightChecker class that handle all the innovation

eaef146

pre-flight checks. Add simple unit testing Use bitmask instead of general flag to have more granularity

PreFlightChecker: use setter for the innovations to check instead of …

f26ea57

…sending booleans in the update function This makes it more scalable as more checks will be added

ekf: Use booleans instead of bitmask for ekf preflt checks

d303e77

Rename "down" to "vert"

bresch force-pushed the dev-ekf-prearm branch from cd16644 to d303e77 Compare October 21, 2019 14:30

bresch commented Oct 21, 2019

View reviewed changes

msg/estimator_status.msg Show resolved Hide resolved

dagar requested a review from a team October 21, 2019 16:35

Merge branch 'master' into dev-ekf-prearm

4de3060

dagar approved these changes Oct 22, 2019

View reviewed changes

bresch merged commit 549fb0d into master Oct 22, 2019

bresch deleted the dev-ekf-prearm branch October 22, 2019 14:22

This was referenced Oct 25, 2019

Release 1.10 release branch cherry picks since beta4 #13273

Closed

Release 1.10 release branch cherry picks since beta4 (take 2) #13277

Merged

MaEtUgR mentioned this pull request Oct 30, 2019

Commander: start pulling arming related parts into separate folder #13224

Merged

dagar mentioned this pull request Nov 5, 2019

Moving towards multi-sensor-per-same-measurement fusion PX4/PX4-ECL#652

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

ekf2_main - Add optical flow innovation pre-flight check #13036

ekf2_main - Add optical flow innovation pre-flight check #13036

bresch commented Sep 27, 2019 •

edited by AuterionWrikeBot

Loading

mhkabir Sep 27, 2019

bresch Oct 1, 2019

bkueng left a comment

bresch commented Oct 1, 2019

bkueng commented Oct 2, 2019

priseborough commented Oct 3, 2019

priseborough commented Oct 3, 2019

CarlOlsson commented Oct 3, 2019

bresch commented Oct 16, 2019

CarlOlsson commented Oct 16, 2019

jorge789 commented Oct 21, 2019 •

edited by Tony3dr

Loading

jorge789 commented Oct 21, 2019 •

edited by Tony3dr

Loading

bresch commented Oct 22, 2019

bresch commented Oct 22, 2019

	bool doing_ne_aiding = control_status.flags.gps \|\| control_status.flags.ev_pos;
	bool doing_hpos_aiding = control_status.flags.gps \|\| control_status.flags.ev_pos;

ekf2_main - Add optical flow innovation pre-flight check #13036

ekf2_main - Add optical flow innovation pre-flight check #13036

Conversation

bresch commented Sep 27, 2019 • edited by AuterionWrikeBot Loading

mhkabir Sep 27, 2019

Choose a reason for hiding this comment

bresch Oct 1, 2019

Choose a reason for hiding this comment

bkueng left a comment

Choose a reason for hiding this comment

bresch commented Oct 1, 2019

bkueng commented Oct 2, 2019

priseborough commented Oct 3, 2019

priseborough commented Oct 3, 2019

CarlOlsson commented Oct 3, 2019

bresch commented Oct 16, 2019

CarlOlsson commented Oct 16, 2019

jorge789 commented Oct 21, 2019 • edited by Tony3dr Loading

jorge789 commented Oct 21, 2019 • edited by Tony3dr Loading

bresch commented Oct 22, 2019

bresch commented Oct 22, 2019

bresch commented Sep 27, 2019 •

edited by AuterionWrikeBot

Loading

jorge789 commented Oct 21, 2019 •

edited by Tony3dr

Loading

jorge789 commented Oct 21, 2019 •

edited by Tony3dr

Loading