RTK GPS heading integration #497
Conversation
EKF/gps_yaw_fusion.cpp
Outdated
| @@ -0,0 +1,433 @@ | |||
| /**************************************************************************** | |||
| * | |||
| * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. | |||
|
@bkueng I have doen some testing using the d9bc48f9-3270-4ff9-8141-c33a006f9f9d.ulg log file posted here: PX4/PX4-Autopilot#10284 The heading measurement is updated at a lower rate by the driver which is holding the last measurement for subsequent frames instead of setting the value to NAN. This results in repeated fusion of the same data and large innovation transients when the vehicle yaws:
Other than that issue, the fusion appears to be working as expected.
|
priseborough
force-pushed
the
pr-ekfGpsYaw
branch
from
c36b566 to
4eb5b4a
Compare
|
Rebased and cleaned commit history. Have included a non-functional clean up change |
|
I will wait until PX4/PX4-Autopilot#10346 is merged and we can get some new log data for replay testing. |
|
@priseborough PX4/PX4-Autopilot#10346 is merged. |
|
PX4/PX4-Autopilot#10346 has been merged. This PR can now be tested using PX4/PX4-Autopilot#10355 |
priseborough
force-pushed
the
pr-ekfGpsYaw
branch
from
4eb5b4a to
6d12246
Compare
priseborough
force-pushed
the
pr-ekfGpsYaw
branch
from
115b508 to
6c9e669
Compare
|
@bkueng Are you able to get another test log using PX4/PX4-Autopilot#10355? The change to PX4/PX4-Autopilot#10355 requested by the reviewers changed the GPS uORB topic and the previous logs will no longer replay. Also it now includes the dual receiver support. |
|
I probably won't go out testing this week again, but I can do it next week. |
|
Test log here: https://logs.px4.io/plot_app?log=9bdbab10-cb54-4f9d-85e6-0b53f338235a Figure showing EKF yaw, GPS yaw and EKF yaw innovations:
Same figure from replay:
Same figure with heading_offset hard coded to correct value of radians(90) to replace 0 value in test log caused by PX4/Firmware bug fixed here
Effect of changing the heading_offset value was minimal in this example. |
|
@RomanBapst or @bresch Please complete review and approval / required changes when available. |
The parameter used to control the maximum dead reckoning time had 'gps' in the parameter name which was confusing because it was used for all measurement types capable of constraining horizontal velocity error growth. The parameter variable has been renamed and the documentation for it improved. The parameter used to control the maximum time since fusing a measurement before the measurement is considered to be not contributing to aiding had misleading documentation which has been updated.
Heading data is assumed to be from a dual antenna array at a specified yaw angle offset in body frame, but with the heading data already corrected for antenna offset. The offset is required to apply the correct compensation for combined rotations and to determine when the yaw observation has become badly conditioned.
Double precision accuracy is not required for this operation.
priseborough
force-pushed
the
pr-ekfGpsYaw
branch
from
6c9e669 to
688da37
Compare
|
Rebased (clean, no conflicts). |
|
@RomanBapst @bresch please review and approve if good |
|
|
||
| if (resetGpsAntYaw()) { | ||
| // flag the yaw as aligned | ||
| _control_status.flags.yaw_align = true; |
There was a problem hiding this comment.
@priseborough This is a duplicate, you have the same thing below.
| float PH[4]; | ||
| _heading_innov_var = R_YAW; | ||
|
|
||
| for (unsigned row = 0; row <= 3; row++) { |
There was a problem hiding this comment.
@priseborough Any reason you are using unsigned for the row but uint8_t for the col?
There was a problem hiding this comment.
its a hang over from early optimisation tests that showed slightly reduced execution times if the inner loop index type was unsigned instead of uint8_t.
EKF/gps_yaw_fusion.cpp
Outdated
| healthy = false; | ||
|
|
||
| // update individual measurement health status | ||
| _fault_status.flags.bad_mag_hdg = true; |
There was a problem hiding this comment.
@priseborough Technically the term 'mag_hdg' can be a bit confusing here, no? Not sure though if it's worth renaming it to just 'bad_hdg'.
There was a problem hiding this comment.
I think thats worth doing now.
This flag now reports on fusion of data that is not from a magnetometer.
|
@RomanBapst Please review changes pushed in response to reviewer suggestions. TXS. |
Overview
Added in response to PX4/PX4-Autopilot#10284
Requires PX4/PX4-Autopilot#10355
These changes add support for use of the GPS measured yaw data that is available here: https://github.com/PX4/Firmware/blob/master/msg/vehicle_gps_position.msg#L35
This data is derived from a dual antenna array which requires knowledge of the yaw angle of the antenna array to be able to utilise the data correctly due to the flipping of the yaw angle that occurs when the line that passes through both antennas goes through the vertical.
New yaw angle fusion and reset methods that takes the yaw offset of the antenna array into account have been added.
There is currently no accuracy estimate available for the GPS heading message data. This implementation uses the same yaw observation noise as set by the EKF2_HEAD_NOISE parameter.
Testing
Test with PX4/PX4-Autopilot#10355
Testing this branch requires settig EKF2_AID_MASK = 129 and EKF2_MAG_TYPE = 5. That will force the EKF to wait until GPS yaw is available before performing the yaw alignment. If EKF2_MAG_TYPE = 0, then the yaw will be set using the magnetomer intially and then reset to the GPS yaw when it becomes available.
Logs starting at boot or a minimum of 20 seconds prior to flight are required to test this feature using the replay facility.