EKF: Problem with quaternion covariance reset #541
Comments
|
I've derived equations that should enable the yaw uncertainty to be increased. This will eliminate the need for a all of nothing reset of quaternion covariances following a reset of the yaw angle. See https://github.com/PX4/ecl/tree/pr-ekfYawCovReset In the process I found some inconsistencies with the way resets were being handled in different places in code that could affect the switchover between position aiding sources or when recovering from a reset following takeoff near a large magnetic anomaly. This is related to #510 This work will be continued in the new year. |
priseborough
changed the title
|
Further investigation of the code showed that this part of a wider issue that can affect all vehicle types that need to use an in-air reset to recover from a ground based magnetic yaw error. This is due to:
Fundamentally the exisiting approach to handling the reset of covariances after a yaw reset was 'all or nothing'. The investigation also revealed that the reset of the yaw angle to enable recovery from ground based magnetic anomally is conditional on switching to 3-axis fusion and can therefore fail to execute depending on the setting of EKF2_MAG_TYPE and the flight profile. A PR to fix this has been submitted #544. A PR will be submitted following further testing. To assist with this a modification to the sitl_gazebo simulaton is being used that includes a crude model of a ground based magnetic anomally - see https://github.com/priseborough/sitl_gazebo/tree/earthFrameMagError |
|
SITL log from preliminary testing of the changes here: https://logs.px4.io/plot_app?log=2b4630a4-06d7-48e3-9e3f-c67eee1f01f5 |
|
I was able to simulate a large tilt angle at the time of the yaw reset by using the 'make posix_sitl gazebo_tailsitter' option, however this did require disabling the "Preflight Fail: Airspeed Sensor stuck" check in the commander and changing the step size and real time update rate in tailsitter.world to 250 Hz, eg <max_step_size>0.004</max_step_size> Result with only critical yaw reset bug fix applied: https://logs.px4.io/plot_app?log=7e5f40a7-00d7-49c7-a0a6-8d422144a907. Note that the reset has the effect of reducing overall angular uncertainty (sum of variances), which is not the correct result. Innovations: Quaternion Variances: Test with all https://github.com/PX4/ecl/tree/pr-ekfYawCovReset changes: https://logs.px4.io/plot_app?log=69dd8220-570d-4897-be79-e2290eedcbd7 Note that the q(0) term increases when the reset occurs, but the other terms are largely unmodified. This makes sense for a vehicle that has the X axis pointing up with an additional yaw uncertainty. Innovations: Quaternion Variances: |
|
@priseborough when testing this branch I found this issue |
|
@priseborough As can be seen the variance for the q[0] and q[3] is increased which is correct.
Are you sure about this? As far as I know standard PX4 uses a convention for tailsitters such that the coordinate frame is similar to that of a quad in hover. I.e. in cruise the tailsitter flies along -z direction. Without doing a hand held test with a real system I am not sure how to test the reset at high pitch angles in SITL. However, I pulled in the changes to our code base and this is the result
|
|
@priseborough I then converted the quaternion covariance matrix to an Euler covariance matrix following these equations However, the result is not as expected, the blue and green lines overlap so the largest increase in variance is in the middle diagonal element (pitch). |
|
However if you plot the quaternion state variances, the biggest increase is in q3 and less for q0 with q1 and q2 variance increase being negligible which is consistent with an increase in yaw uncertainty for a vehicle that is close to zero roll and pitch.
|
|
Yes, which indicates that the code is working as intended. I just need to figure out why the conversion to Euler angles does not give the expected result |
|
@CarlOlsson I think it's related with the 321 and 312 variations that Paul spoke of and I think I know how to handle those on the covariance law. I will do an update later today. |
|
Yes me too, I just did a test with a quad. Hover for some time, increase heading noise, hover, decrease heading noise The middle element in the covariance matrix is definitely yaw and not pitch |
|
Okey, so the confusion around the quaternion convention is sorted out in #543 What is left for this is to verify that the yaw uncertainty is increasing correctly for all attitudes, e.g. at pitch 90 degrees.
This seems correct. I then simulated a flight where the vehicle flew in a square (standard attitude, so body z facing down). In the middle of every leg I triggered This is the result As can be seen the variance from the 321 sequence seems correct. but for the 312 sequence something is strange, for the east and west direction no increase of uncertainty is observed |
|
For reference, here is the G matrices |
|
@CarlOlsson have you tried to use the same G matrix for both 321 and 312 rotation orders? I tried to use the same for these and I seemed to get the same results on plots. Update: nevermind, sorry. I used for 321 and 123, not 312. We probably need to face that case as well, though I am not sure how we identify the rotation order on the function so to apply the proper G matrix. |
I don't understand what you mean. The Jacobian (G matrix) is defined by the way you calculate the yaw angle which depends on the Euler sequence. How can you have different Euler sequences with the same Jacobian? |
|
I can't really tell but for the 321 and 123 orders, applying the same G matrix, I get the same results. I can try to verify that on math. But I suppose that doesn't apply for a 312 rotation case. |
You can probably use the same method as here For the logfile mentioned above, here is the raw quaternions North West South |
Yeah actually @mhkabir pointed me that as well. I'm just trying to figure how it would apply here. |
|
I tested this code again, this time on a sitl quad. Then at every waypoint I triggered uncorrelateQuatStates() and increaseQuatYawErrVariance() two times This time I sometimes got Comparing the yaw variance calculated using the 321 and 312 Euler sequence looks like this As can be seen they give similar result for the north/south direction but not for east/west. I don't know why at the moment. yaw for the 312 calculation is given by and the G matrix is: |
|
Where are you getting the derivation for the conversion from a quaternion rotation to a 312 Tait-Bryan sequence and the derivation for the associated partial derivative matrix? |
|
The conversion from quaternion rotation to a rotation matrix comes from here, and then to 312 Tait-Bryan yaw comes from here The derivation for the partial derivative matrix comes from the following python script |
|
I've done further investigation today and the "EKF mag yaw fusion numerical error - covariance reset" could be the result of setting the off-diagonals to zero and having fusion take place before the next covariance prediction. How were you triggering uncorrelateQuatStates() and increaseQuatYawErrVariance() ? I will add code to the branch so that the fusion is blocked until after the next covariance prediction. Using Matlab I independently derived the Tait-Bryan first rotation (yaw) in a 312 sequence as a function of the quaternions and got yaw = atan2(2q0q3 - 2q1q2, q0^2 - q1^2 + q2^2 - q3^2); d_yaw/d_q0 = (((2q3)/(q0^2 - q1^2 + q2^2 - q3^2) - (2q0*(2q0q3 - 2q1q2))/(q0^2 - q1^2 + q2^2 - q3^2)^2)(q0^2 - q1^2 + q2^2 - q3^2)^2)/((2q0q3 - 2q1*q2)^2 + (q0^2 - q1^2 + q2^2 - q3^2)^2); d_yaw/d_q1 = -(((2q2)/(q0^2 - q1^2 + q2^2 - q3^2) - (2q1*(2q0q3 - 2q1q2))/(q0^2 - q1^2 + q2^2 - q3^2)^2)(q0^2 - q1^2 + q2^2 - q3^2)^2)/((2q0q3 - 2q1*q2)^2 + (q0^2 - q1^2 + q2^2 - q3^2)^2); d_yaw/d_q2 = -(((2q1)/(q0^2 - q1^2 + q2^2 - q3^2) + (2q2*(2q0q3 - 2q1q2))/(q0^2 - q1^2 + q2^2 - q3^2)^2)(q0^2 - q1^2 + q2^2 - q3^2)^2)/((2q0q3 - 2q1*q2)^2 + (q0^2 - q1^2 + q2^2 - q3^2)^2); d_yaw/d_q3 = (((2q0)/(q0^2 - q1^2 + q2^2 - q3^2) + (2q3*(2q0q3 - 2q1q2))/(q0^2 - q1^2 + q2^2 - q3^2)^2)(q0^2 - q1^2 + q2^2 - q3^2)^2)/((2q0q3 - 2q1*q2)^2 + (q0^2 - q1^2 + q2^2 - q3^2)^2); |
|
I compared these equations to the previous results from sympy, the outcome is identical Green and orange overlap The reset was triggered in ekf2_main.cpp from the console This was added before the _ekf.update() function so at least one covariance prediction step should happen before a measurement fusion is triggered. |
|
OK, so I will focus the investigation for the time being on the cause of the 'EKF mag yaw fusion numerical error - covariance reset' events. One thing to try is to ensure the covariance prediction runs before the next fusion step. I would like to be able to reproduce your results in my testing using the same mission plan and firmware modifications. |
|
Thank you to @CarlOlsson and Sebastian for spotting a sign error that was introduced when the auto-code had been cleaned up by hand in the increaseQuatYawErrVariance function. The https://github.com/PX4/ecl/tree/pr-ekfYawCovReset branch has been rebased has been rebased and the error fixed here: A compile error introduced when the a rebase conflict was resolved has also been fixed - see |
|
@priseborough As discussed I pushed two commits to this branch. |
|
@CarlOlsson I've pushed three commits to the branch which should address the issue of unstable declination immediately following a field reset when using 3-axis fusion and the loss of declination covariance information when switching in and out of 3-axis fusion in auto mode in into 3-axis fusion with EKf2_MAG_TYPE = 0 |
|
Currently investigating the first switch into 3-axis fusion with EKF2_MAG_TYPE=0. The mag field variances appear to be initialising to zero.
|
|
Actually not only this problem seems to be initializing the covariances to zero There is also a problem when we call fuseDeclination() in resetMagYaw(). When we call this function the first time we activate 3d fusion we also call fixCovarianceErrors(), and here we set the mag field state covariances to zero since _control_status.flags.mag_3D is false |
|
The covariance reset to 0 bug has been fixed here: SITL log: https://logs.px4.io/plot_app?log=39f319e4-847f-467c-9056-78fb917c0515 |
|
@priseborough I've put up a PR for easier reference to that commit: #556 |
|
@priseborough Some comments to the updated code in this branch
|
|
@CarlOlsson I will try 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. Discussion continued in #544. |
|
Fixed on master. |
When doing a yaw reset using the Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) method (see https://github.com/PX4/ecl/blob/master/EKF/ekf_helper.cpp#L1429) if the vehicle has a large roll or pitch angle (eg tailsitter), the use of a rotation vector variance to represent the uncertainty does not enable the uncertainty in yaw to be changed without affecting the tilt (roll/pitch) variance.
A better method is required.
The text was updated successfully, but these errors were encountered: