EKF2 fix skipping magnetometer axes upon other axis failure #13921
Conversation
Ensure that if fusion checks for an axis fails, fusion will not be perfomred for all axes.
|
@peterbarker is going to review this PR |
|
ping again for @peterbarker to review |
There was a problem hiding this comment.
Code review only, I haven't test yet on a failure
We don't have copy-paste error. It is now done similary to EKF3, and make me wonder why we trying to do the fusion each axis twice on EKF3 since
for (mag_state.obsIndex = 0; mag_state.obsIndex <= 2; mag_state.obsIndex++)
is not remove on SelectMagFusion() like this patch does ?
Anyway,
The variable initialisation on start of FuseMagnetometer could be simplified. But this put forward a difference with the previous code.
Previously, we were doing axis X fusion with stateStruct data and other axis with mag_state data. Now we are using only stateStruct data. Is that the correct behavior ? If that is correct, we should create and initialize the date directly with stateStruct and not mag_state. The same behavior exist in EKF3.
All the perf checker were removed.
before : we used a loop for H_MAG setting to 0, we are now using a memset. The use is correct.
Then we are doing a loop for the Jabcobian calculation for each axis and at the end of each axis setting magFusePerformed=true. This is still necessary to correctly calculate the fusion health and record which axis is going wrong.
we zero the attitude error state and apply it to rotate the stateStruct.quat (L693) that seems unnecessary and not done in EKF3
|
my conserns about EKF3 issues are fixed in #13906 |
|
@peterbarker bump |
I don't think that's correct. The declaration sets up a reference; from q0 to the The critical thing here is that we are initialising Looking further, we could eliminate |
| } | ||
| } | ||
|
|
||
| // fuse the three magnetometer componenents using sequential fusion of each axis |
There was a problem hiding this comment.
| // fuse the three magnetometer componenents using sequential fusion of each axis | |
| // fuse the three magnetometer components using sequential fusion of each axis |
... or just remove the comment entirely as it now assumes it knows what that function does, meaning it can get out-of-date.
| if (obsIndex == 0) | ||
| { | ||
| // calculate observation jacobians | ||
| memset(&H_MAG, 0, sizeof(H_MAG)); |
There was a problem hiding this comment.
Future cleanup could make H_MAG declared within the for loop, which would remove the need for getting these memsets correct.
There was a problem hiding this comment.
the curly bracket should be on line 447..
| hal.util->perf_begin(_perf_test[5]); | ||
| // set flags to indicate to other processes that fusion has been performed | ||
| // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step | ||
| magFusePerformed = true; |
There was a problem hiding this comment.
Future cleanup can move this assignment elsewhere; there's no reason to do it on each axis.
|
needs testing with replay |
|
We generally agreed on the call that this can go in but Tridge is keen that we run it through replay before merging. |
|
To be clear, I volunteered to run this through Replay and merge it if nothing unexpected happens. |
thanks Peter! |
|
I've now run this through Replay on a lots of logs - where . I haven't seen it produce anything but byte-for-byte identical output logfiles. |
|
Merged, thanks! |
Addresses #13912
Ensures that if fusion checks for an axis fails, fusion will not be performed for all axes. The only exceptions are if the failure is at the covariance update step due to a badly conditioned covariance matrix, in which case fusion for that and not completed axes will be aborted, however this is an extremely rare occurrence.