Pr ekf yaw cov reset #556
Pr ekf yaw cov reset #556
Conversation
This is necessary after a quaternion reset to prevent incorrect attitude corrections with subsequent observation fusions.
Use a new method that preserves the roll and pitch information and adds the uncertainty for yaw only. Ensure that correlation information to non-quaternion states is removed when a reset occurs to prevent fusion of subsequent observations (e.g. GPS) causing incorrect yaw.
…rrVariance() Signed-off-by: CarlOlsson <[email protected]>
…ditioned covariance matrix Signed-off-by: CarlOlsson <[email protected]>
Ensures that each time the earth field covariance and variance data is reset, that the off-diagonal elements containing earth field declination angle certainty is restored.
…axis fusion. In the case where the EKF is switching between 3-axis and heading fusion, off-diagonal elements containing the correlation between N,E components of the earth field were being lost on each switch event. These elements contained information about the declination uncertainty and should be preserved.
…e-use Prevents use of _saved_mag_ef_covmat and _saved_mag_bf_variance before being initialised or when stale.
Simplify calling so that it is only called in two ways: 1) Immediately before 3-axis mag fusion if not called since the last earth field covariance reset so that the earth field declination information can be formed. 2) Immediately after 3-axis mag fusion otherwise.
|
@CarlOlsson I have implemented another approach to simplify the way fuseDeclination() is only called, either immediately before fuseMag() if it hasn't been called since the last yaw reset or immediately after if _control_status.flags.mag_dec is true. This will simplify the overall logic. See commit: See SITL log: https://logs.px4.io/plot_app?log=1bd258f2-7bef-4b9a-8cc8-c13190fb89c1 |
|
The fuseDeclination() function was being called unnecessarily on each re-entry into 3-axis fusion when auto switching was enabled. Fixed here: SITL log here: https://logs.px4.io/plot_app?log=59890a0b-542f-4d11-b978-f3cebe3bae2e |
|
MC flight test logs. A ground fixed bar magnet was placed near the vehicle to swing the magnetic heading prior to takeoff and trigger a significant inflight yaw reset when the vehicle climbed above 1.5m, eg
Note: for this test the internal magnetometer sensors were disabled using the CAL_MAG1_EN, CAL_MAG2_EN, CAL_MAG3_EN parameters. These are the mag yaw and mag field innovations from each flight https://logs.px4.io/plot_app?log=233b88b5-0165-44c3-8106-d1e0069f7e8f These are the yaw angles calculated from the reset delta quaternion published in the vehicle_attitude topic. The ground based magnetic anomaly was over 45 deg:
No perturbation in flight behaviour was observed. The earth field state mag variances show the reset and subsequent switching between heading and 3-axis fusion in flight working as expected:
Quaternion covariances change as expected at the reset with the q0 and q3 terms experiencing the largest increase as expected for a yaw reset with a vehicle that is close to zero pitch/roll:
Further testing with logging from boot will be performed with a magnet placed on the airframe to swing magnetic heading and test the ability of the EKF to learn the body fixed mag field error when toilet bowling starts. If required, these logs can then be used via replay to make changes required to increase the speed at which the estimator recovers. |
|
@bkueng I think @priseborough has a logging issue here - could you follow up on PX4 slack? Thanks! |
|
Logging issue has been resolved. it was a problem with my log file processing. |
|
Testing with a small neodymium button magnet placed on top of the RH rear frame hex fastener generated a ~300mG offset in the compass. Effect of large mag hard iron error with moderate initial yaw error Here is the measured mag field without the magnet and the vehicle level and pointing to magnetic East (log https://logs.px4.io/plot_app?log=9fbe4146-2ddb-44f5-9a8b-2818ab3e0449):
Here it is with the magnet attached (logs https://logs.px4.io/plot_app?log=4e883d49-aad6-4cbf-8a19-c51c29f957cc and https://logs.px4.io/plot_app?log=4a1427cd-e6af-4421-a720-5b83e2b1190d):
When starting pointing at magnetic East, the resultant initial yaw error at that orientation was 44 degrees and the vehicle 'toilet bowled' moderately after takeoff, but was able to learn the body frame error and recover without triggering a failsafe:
Body field estimates: Earth field estimates: Magnetic yaw and field innovations: GPS NE velocity innovations: Other Test Logs The same test was repeated with the initial heading at magnetic 180, 270 and 360 degrees. Log data with magnetometer disturbance: Initial Yaw 180 : https://logs.px4.io/plot_app?log=39a161d0-65f7-4e65-a501-773759e4ae54, https://logs.px4.io/plot_app?log=0d60d575-286a-4b38-9cc5-0c1278158810 Data without magnetometer disturbance: Initial Yaw 180 : https://logs.px4.io/plot_app?log=c3f762ac-2b07-4fd1-84f7-55ed0592d35a Effect of large hard iron error with correct initial heading At initial yaw 360, because the error in the X field was aligned with the horizontal magnetic field vector, the initial heading error was small and no external behaviour changes were observed. The EKF was able to learn the offset transparently to the operator: Body Magnetic Field Estimates: Earth Magnetic Field Estimates: The declination angle of the Earth field states was stable, and started recovering from the initial 0.1 rad (6 deg) transient estimation error: Effect of large hard iron error with larger initial yaw error At an initial magnetic heading of 180 deg (190 deg wrt true north) large velocity innovations and rapidly accelerating toilet bowling triggered a failsafe reversion into ALTCTL. The initial magnetic yaw error was in excess of 125 deg and the filter was unable to recover with initial errors so large that the initial earth field declination estimate flipped though 180 degrees. eg:
At 270 deg initial yaw angle, although the velocity innovations and toilet bowling were high enough to trigger a failsafe reversion into ALTCTL, the field estimates did converge, eg:
and the filter regained usable velocity estimates within 40 seconds of takeoff: Recommendations Protection against the declination flipping sign due to large magnetometer errors is required. The logs gathered will be used with replay can now be used to test methods. |
|
@bkueng The ekf2 replay appears to be running, but the generated log only contains headers and parameters, not data. I am working from v1.9.0-beta1 at commit 504372f plus the commit to point to the ecl pr-ekfYawCovReset branch at commit 1977b11. The same problem also occurs with unmodified v1.9.0-beta1 code. Input log: Output log: Console output: |
|
@bkueng I've done done some further investigation and have confirmed that the ekf2 module is running this line https://github.com/PX4/Firmware/blob/master/src/modules/ekf2/ekf2_main.cpp#L1758 but the uORB data is not being detected by the logger module here: https://github.com/PX4/Firmware/blob/master/src/modules/logger/logger.cpp#L1157 because log_message_updated always returns false. |
|
Does this also handle #557? |
…timates Fusion with large initial magnetometer biases errors can result in the the NE earth field states reducing in magnitude and effectively flipping sign. EKF: Move declination state limiting into a separate function EKF: Limit NE mag states after each 3-axis mag fusion EKF: Fix bug in mag field strength look-up scale factor
priseborough
force-pushed
the
pr-ekfYawCovReset
branch
from
d0d76b1 to
c166968
Compare
|
Commit As per previous testing a magnet was attached to the vehicle and it was aligned and flown initially in POSCTL mode at four initial headings relative to magnetic North: Yaw 000 : https://review.px4.io/plot_app?log=d3aaa146-eab7-460d-8e77-0af368a961d5
Horizontal strength and declination of estimated earth field were converging to reasonable values:
Yaw 090 : https://review.px4.io/plot_app?log=b3cab03a-d5f9-4d39-b738-fd08eaf86483
Horizontal strength and declination of estimated earth field were converging to reasonable values:
Yaw 180 : https://review.px4.io/plot_app?log=2ed74812-d284-4e94-836c-8e1ca71fcae2
Horizontal strength and declination of estimated earth field were converging to reasonable values:
Yaw 270 : https://review.px4.io/plot_app?log=875767b2-174d-4cd5-ba3f-d413cc0075b7
Horizontal strength and declination of estimated earth field were converging to reasonable values:
The magnet was reversed to reverse the sign of the initial offset and the tests repeated Results were similar taking into account the changed magnetic offset. Yaw 000 : https://review.px4.io/plot_app?log=d6925cc4-ae9a-4e08-8f87-c5d7dd4bec03 Field estimation was degraded, but stayed within limits: Yaw 090 : https://review.px4.io/plot_app?log=7b0c717c-74d1-44e8-9644-47c877125211 Field estimates were within limits and converging to reasonable values Yaw 180 : https://review.px4.io/plot_app?log=4e9e3a14-3feb-4b3d-bcde-96bd5685d965 Field estimates were within limits and converged rapidly to reasonable values Yaw 270 : https://review.px4.io/plot_app?log=a22f4a9b-45ef-4bdf-b597-1a17ca052b14 Field estimates were within limits and converged rapidly to reasonable values |
|
@dagar are you able to provide some advice on how to resolve the CI error: http://ci.px4.io:8080/blue/organizations/jenkins/PX4%2Fecl/detail/pr-ekfYawCovReset/16/pipeline/ |
priseborough
requested review from
CarlOlsson
and removed request for
priseborough
Working on a proper fix for Jenkins here - #564 |
|
I replayed 18 logfiles with and without this PR. As can be seen below the estimated magnetic declination is more stable and the inclination, field strength and body field biases are equally stable as on master
|
|
Tried to test the reset logic for fw in SITL ( |
|
@CarlOlsson I was able to run it in my Ubuntu 16.04 build environment and confirm that the patch
Log is here: https://review.px4.io/plot_app?log=ac6f88f8-a8b6-4a2d-8e3e-e70aadc604b7 |
|
@priseborough Could you approve this PR once you consider it complete and ready for on-vehicle flight testing? |
|
Could everyone please converge on testing the PX4/Firmware side ecl update? PX4/PX4-Autopilot#10986 |
Work in progress started in response to #541. Refer to #541 for previous discussion and testing.