Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[ECL] Moving towards multi-sensor-per-same-measurement fusion #13127

Merged
merged 5 commits into from
Dec 5, 2019

Conversation

kamilritz
Copy link
Contributor

@kamilritz kamilritz commented Oct 8, 2019

This ECL PR aims to provide unambiguous logging of velocity and position measurement updates in the EKF2 estimator.

To enable this, we need to provide the right uorb messages on the Firmware side. Making the measurements sensor specific would increase the number of fields in ekf2_innovations.msg.
To avoid this we split the message in to sensor specific innovations, innovation variances and innovation test ratios.
These messages have different use-cases and can be enabled accordingly.

Logs of several test flights can be found in the ECL PR.

The ekf2_innovations.msg is marked as deprecated. The fields are replaced according to the following scheme:

ekf2_innovations.vel_pos_innov -> estimator_innovations.[gps_hvel, gps_vvel, gps_hpos, gps_vpos, ev_hvel, ev_vvel, ev_hpos, ev_vpos, fake_hvel, fake_vvel, fake_hpos, fake_vpos ]
ekf2_innovations.mag_innov -> estimator_innovations.mag
ekf2_innovations.heading_innov -> estimator_innovations.heading
ekf2_innovations.airspeed_innov -> estimator_innovations.airspeed
ekf2_innovations.beta_innov -> estimator_innovations.beta
ekf2_innovations.flow_innov -> estimator_innovations.flow
ekf2_innovations.hagl_innov -> estimator_innovations.hagl
ekf2_innovations.vel_pos_innov_var -> estimator_innovation_variances.[gps_hvel, gps_vvel, gps_hpos, gps_vpos, ev_hvel, ev_vvel, ev_hpos, ev_vpos, fake_hvel, fake_vvel, fake_hpos, fake_vpos ]
ekf2_innovations.mag_innov_var -> estimator_innovation_variances.mag
ekf2_innovations.heading_innov_var -> estimator_innovation_variances.heading
ekf2_innovations.airspeed_innov_var -> estimator_innovation_variances.airspeed
ekf2_innovations.beta_innov_var -> estimator_innovation_variances.beta
ekf2_innovations.flow_innov_var -> estimator_innovation_variances.flow
ekf2_innovations.hagl_innov_var -> estimator_innovation_variances.hagl
ekf2_innovations.output_tracking_error -> estimator_status.output_tracking_error
ekf2_innovations.drag_innov -> estimator_innovations.drag
ekf2_innovations.drag_innov_var -> estimator_innovation_variances.drag
ekf2_innovations.aux_vel_innov -> estimator_innovations.[aux_hvel, aux_vvel]
not existing -> estimator_innovation_variances.[aux_hvel, aux_vvel]

@kamilritz kamilritz force-pushed the ecl_refactoring_velpos branch from 346b7d3 to cd92cc8 Compare October 10, 2019 13:12
@kamilritz kamilritz changed the title [WIP][ECL] Moving towards multi-sensor-per-same-measurement fusion [ECL] Moving towards multi-sensor-per-same-measurement fusion Oct 15, 2019
@kamilritz kamilritz force-pushed the ecl_refactoring_velpos branch from 0c67074 to 11a8d41 Compare October 15, 2019 06:35
@kamilritz kamilritz marked this pull request as ready for review October 15, 2019 06:35
@kamilritz
Copy link
Contributor Author

@dagar This is running out of space on fmu-v2 :)
Do you have an idea how we can get this in?

@bresch
Copy link
Member

bresch commented Oct 15, 2019

Looks good

@TSC21
Copy link
Member

TSC21 commented Oct 15, 2019

@TSC21 TSC21 requested review from mhkabir, dagar and jkflying October 15, 2019 08:54
Copy link
Member

@TSC21 TSC21 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@kamilritz is there any reason ware creating 2 new messages for this? Why not just use ekf2_innovations.msg and add #TOPICS aliases for the other two, as they seem to have the exact same fields?

@TSC21
Copy link
Member

TSC21 commented Oct 15, 2019

@kamilritz is there any reason ware creating 2 new messages for this? Why not just use ekf2_innovations.msg and add #TOPICS aliases for the other two, as they seem to have the exact same fields?

This would be something like: (ekf2_innovation.msg)

uint64 timestamp	# time since system start (microseconds)

# GPS
float32[2] gps_hvel	# horizontal GPS velocity innovation variance
float32    gps_vvel	# vertical GPS velocity innovation variance
float32[2] gps_hpos	# horizontal GPS position innovation variance
float32    gps_vpos	# vertical GPS position innovation variance

# External Vision
float32[2] ev_hvel	# horizontal external vision velocity innovation variance
float32    ev_vvel	# vertical external vision velocity innovation variance
float32[2] ev_hpos	# horizontal external vision position innovation variance
float32    ev_vpos	# vertical external vision position innovation variance

# Height sensors
float32 rng_vpos	# range sensor height innovation variance
float32 baro_vpos	# barometer height innovation variance

# Auxiliary (horizontal) velocity
float32[2] aux_hvel	# horizontal auxiliar velocity innovation variance from landing target measurement
float32[2] flow		# flow innvoation variance

# Various
float32 heading		# heading innovation variance
float32[3] mag		# earth magnetic field innovation variance
float32[2] drag		# drag specific force innovation variance
float32 airspeed	# airspeed innovation variance
float32 beta		# synthetic sideslip innovation variance
float32 hagl		# height of ground innovation variance

#TOPICS ekf2_innovations ekf2_innovation_variances ekf2_innovation_test_ratios

Also, please add those aliases to the RTPS ID's list.

@kamilritz
Copy link
Contributor Author

@TSC21 That is a nice idea. The innovation_test_ratios fields are not exactly the same. But we can do this for innovation and innovation variances.

@TSC21
Copy link
Member

TSC21 commented Oct 15, 2019

@TSC21 That is a nice idea. The innovation_test_ratios fields are not exactly the same. But we can do this for innovation and innovation variances.

@kamilritz the only difference I see is on the arrays. The same structure can still be used, but instead of using the entire array, just used the first array index to propagate the test ratio (this can be clarified on a comment).

msg/ekf2_innovations.msg Outdated Show resolved Hide resolved
msg/ekf2_innovations.msg Outdated Show resolved Hide resolved
src/modules/ekf2/ekf2_main.cpp Outdated Show resolved Hide resolved
src/modules/local_position_estimator/sensors/mocap.cpp Outdated Show resolved Hide resolved
src/modules/local_position_estimator/sensors/vision.cpp Outdated Show resolved Hide resolved
src/modules/local_position_estimator/sensors/vision.cpp Outdated Show resolved Hide resolved
@TSC21
Copy link
Member

TSC21 commented Oct 15, 2019

@dagar This is running out of space on fmu-v2 :)
Do you have an idea how we can get this in?

I am not sure how much short we can get on fmu-v2 to get this in :S

msg/tools/uorb_rtps_message_ids.yaml Outdated Show resolved Hide resolved
msg/tools/uorb_rtps_message_ids.yaml Outdated Show resolved Hide resolved
Copy link
Member

@TSC21 TSC21 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

All good for me now

@TSC21 TSC21 self-requested a review October 15, 2019 13:17
@dagar
Copy link
Member

dagar commented Oct 15, 2019

@dagar This is running out of space on fmu-v2 :)
Do you have an idea how we can get this in?

I am not sure how much short we can get on fmu-v2 to get this in :S

There's quite a bit more that can be saved with a little effort. #12601 #12191

@TSC21
Copy link
Member

TSC21 commented Oct 15, 2019

@dagar This is running out of space on fmu-v2 :)
Do you have an idea how we can get this in?

I am not sure how much short we can get on fmu-v2 to get this in :S

There's quite a bit more that can be saved with a little effort. #12601 #12191

Still on time for a release?

TSC21
TSC21 previously approved these changes Oct 15, 2019
Copy link
Member

@LorenzMeier LorenzMeier left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This would break all post-flight tooling due to the uorb format changes - we need to take a more compliant approach for message changes. It is fine to break uORB to some degree, but ulog needs to be a more steady format.

@dannyfpv
Copy link

dannyfpv commented Nov 26, 2019

Tested on pixhawk4 v5 f-450
Flight Card 1

Modes Tested:
Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.

Procedure:
Arm and Takeoff in position mode, after flying for approximately one minute, switched to altitude then stabilized mode.

Notes:
No issues were noted, good flight in general.

Logs:
https://review.px4.io/plot_app?log=a671f8b5-6372-47d1-97dc-e4b4d0a3a379Flight Card 2

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

Procedure
Armed form QGC, Took-off on mission plan mode. Note that commands were sent form QGC (Armed and takeoff). Once all waypoint completed vehicle switched to RTL and landed as expected. Good flight in general.

Note: No issues were noted, good flight in general.

Logs: https://review.px4.io/plot_app?log=38bf258c-29c1-43c2-ba2e-2f1df34e5af4

Flight Card 3
Modes Tested
Position 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 mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint see landing behavior.
Good flight in general.

Note:
No issues were noted, good flight in general.

Logs: https://review.px4.io/plot_app?log=a2b30f1d-1a01-40a6-b305-69ca7692d8cf

Flight Card 4
Modes Tested
Fail safe:The radio control turned off and the vehicle returned and landed correctly.
Tested good.

Logs:
https://review.px4.io/plot_app?log=523e05cc-2d63-460a-8792-518502e1efc1

Tested on PixRacer V4 f-450

Flight Card 1

Modes Tested:
Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.

Procedure:
Arm and Takeoff in position mode, after flying for approximately one minute, switched to altitude then stabilized mode.

Notes:
No issues were noted, good flight in general.

Logs: https://review.px4.io/plot_app?log=1cfb8413-51a8-4493-97c1-61cbdc58048c

Flight Card 2

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

Procedure
Armed form QGC, took-off on mission plan mode, commands were sent form QGC (Armed and takeoff). Once all waypoint completed vehicle switched to RTL and landed as expected. Good flight in general.
Note:
No issues were noted, good flight in general.

Logs:
https://review.px4.io/plot_app?log=48c373d4-f01f-4f45-8c3e-2a253840a0f0

Flight Card 3
Modes Tested
Position 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 mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint see landing behavior.
Good flight in general.

Note:
No issues were noted, good flight in general.

Logs:
https://review.px4.io/plot_app?log=26350564-ae22-46c4-a008-7a193075192a

Flight Card 4
Modes Tested
Fail-safe Test :The radio control turned off and the vehicle returned and landed correctly.
Tested good.

Logs: Software Crash
https://review.px4.io/plot_app?log=e84f88bf-d5eb-498b-95be-e6c9f0d025e9

@Junkim3DR
Copy link

Junkim3DR commented Nov 26, 2019

Tested on NXP FMUK66 v3

Flight Card 1

Flight Card 2

  • Modes Tested
    -Mission Plan Mode (Automated): Good.
    -RTL (Return To Land): Good.

  • Procedure
    Arm and Take off in mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint see landing behavior.

  • Notes
    Vehicle did not land properly.
    While landing, vehicle descended very quickly then stoped at about 3 meters from the ground and ,like a bounce, the vehicle increased altitude to about 10 meters then it proceeded to rush to the ground fast enough for it to bounce off the ground and a really violent way and flipped. (Pilot required to switch "Kill Switch" ON in order to avoid vehicle damaging motors and propellers after the bounce/flip).
    Loaded Master and vehicle behaved as expected, no issues.

  • Log
    https://review.px4.io/plot_app?log=1a70df8e-aeac-47c3-87ac-ad39b73b8e00

Flight Card 3

  • Modes Tested

    • Position 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 mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint see landing behavior.

  • Notes
    Vehicle did not land properly.
    While landing, vehicle descended very quickly then stoped at about 3 meters from the ground and ,like a bounce, the vehicle increased altitude to about 10 meters then it proceeded to rush to the ground fast enough for it to bounce off the ground and a really violent way and flipped. (Pilot required to switch "Kill Switch" ON in order to avoid vehicle damaging motors and propellers after the bounce/flip).
    Loaded Master and vehicle behaved as expected, no issues.

  • Log
    https://review.px4.io/plot_app?log=7ae7fbcb-9531-47d1-9267-dd0bd772e4f4

Tested on Pixhawk Pro v4

Flight Card 1

Flight Card 2

  • Modes Tested
    -Mission Plan Mode (Automated): Good.
    -RTL (Return To Land): Good.

  • Procedure
    Arm and Take off in mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint see landing behavior.

  • Notes
    No issues noted, good flight in general.

  • Log
    https://review.px4.io/plot_app?log=ac657dc9-733b-471b-907c-cd0bab7f772a

Flight Card 3

  • Modes Tested

    • Position 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 mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint see landing behavior.

  • Notes
    No issues noted, good flight in general.

  • Log
    https://review.px4.io/plot_app?log=aab917c9-aa48-42be-a439-b857f7266e1a

Flight Card 4

  • Modes Tested

    • Position Mode: Good.
    • Failsafe RTL (Return To Land): Not returning after RC shutdown.
  • Procedure
    Arm and Take off in position mode, after flying for approximately one minute, turned RC off in order to activate failsafe RTL, wait for vehicle to return and land as expected.

  • Notes
    No issue noted, good flight in general.

  • Log
    https://review.px4.io/plot_app?log=c0a1bc66-9c00-43f3-a03e-431b6bae2613

Copy link
Contributor

@jkflying jkflying left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just some tiny changes, otherwise this side of the PR LGTM

{
const float vel_d_innov = innov.vel_pos_innov[2];
const float vel_d_innov = fmaxf(fabsf(innov.gps_vvel), fabs(innov.ev_vvel));
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

IMO: longer term the preflight checks shouldn't use innovations at all , but rather the covariances of the states. In the mean time this is a conservative check that can be performed, but I think we all agree it isn't optimal.

src/modules/local_position_estimator/sensors/gps.cpp Outdated Show resolved Hide resolved
_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
}

for (size_t i = 3; i < 6; i++) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just as a note, this was broken before

@MaEtUgR
Copy link
Member

MaEtUgR commented Nov 28, 2019

Loaded Master and vehicle behaved as expected, no issues.

@Junkim3DR To see if the reported problems in landing detection are for sure related to this pr and what exactly was different on that particular setup it would help a lot to have the log when you compared with master and had no issues as well. Do you still have this log? Could you upload it?

jkflying
jkflying previously approved these changes Nov 28, 2019
@Junkim3DR
Copy link

Do you still have this log? Could you upload it?

Hi @MaEtUgR, I believe this are the 2 logs I tested with master to see if the issue persisted.

Logs

@Tony3dr
Copy link

Tony3dr commented Nov 28, 2019

@MaEtUgR it's the same setup nothing was changed.

@kamilritz
Copy link
Contributor Author

@Junkim3DR can you please fly the NXP FMUK66 v3 with flight card 2&3 again. Once with this PR and once with current master. Please perform the same flights with both firmware version. Thank you very much.

@mhkabir
Copy link
Member

mhkabir commented Dec 1, 2019

Remaining things before we merge this (we're very close!):

  1. FMU-v2 flash overflow.
  2. Merge ECL PR.
  3. Rebase this on master and update ECL reference.
  4. Make sure land detection failure is not related to changes here.

@kamilritz
Copy link
Contributor Author

  1. Make sure land detection failure is not related to changes here.

@mhkabir any ideas how I can investigate this?

@Junkim3DR
Copy link

Tested on NXP FMUK66 v3

Flight Card 2

Flight Card 3

@kamilritz kamilritz force-pushed the ecl_refactoring_velpos branch from 9428d48 to 8c105d2 Compare December 3, 2019 13:07
@kamilritz
Copy link
Contributor Author

@Junkim3DR Thanks for the new log files. There is definitively something odd. We would need another log file that we can use for EKF replays. Can you flash the newest version of this PR, set SDLOG_PROFILE=2, set SDLOG_MODE="from boot to disarm", fly Flight Card 2 again and upload the log.
Thank you very much.

@kamilritz
Copy link
Contributor Author

@Junkim3DR Which master firmware were you running as reference?

@kamilritz kamilritz force-pushed the ecl_refactoring_velpos branch from 8c105d2 to 48f892a Compare December 3, 2019 16:51
@Junkim3DR
Copy link

Junkim3DR commented Dec 3, 2019

We would need another log file that we can use for EKF replays. Can you flash the newest version of this PR, set SDLOG_PROFILE=2, set SDLOG_MODE="from boot to disarm", fly Flight Card 2 again and upload the log.

Tested on NXP FMUK66 v3

Flight Card 2

Which master firmware were you running as reference?

Master used was latest form QGC the 12th of December 2019, commit: 9b66cbd

@kamilritz
Copy link
Contributor Author

@Junkim3DR Thank you very much!

@mhkabir @dagar Together with @priseborough we tracked down the issue in the first test flights to extensively filtered baro data on the vehicle_air_data. On my test platform and in the newest log files on the NXP FMUK66 platform the baro data seems to be noisy as expected and the weird behavior during landing caused by bad vertical estimates is not noticeable.

I argue that it is okay to merge this now. LGTM!!!! (Reminder: ECL PR has to go first: PX4/PX4-ECL#668)

@priseborough
Copy link
Contributor

I've checked the logs from Mexico testers who were using the standard GPS sensor fit and daa looked OK.

jkflying
jkflying previously approved these changes Dec 5, 2019
@jkflying jkflying dismissed stale reviews from mhkabir and LorenzMeier December 5, 2019 10:27

addressed issues

@jkflying jkflying merged commit d7b9587 into PX4:master Dec 5, 2019
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.