Make the target EKF rate an integer multiple of the IMU rate. This slightly increases the average prediction time step for the EKF from just over 10msec to 12msec, but the variation reduces significantly which makes filter tuning more deterministic.
Improve the algorithm used to adjust the collection time criteria to reduce jitter in the correction.
This is a functionally equivalent. It moves all of the code for the terrain estimator into a single function call from the main filter update, making it clear that it is independent of the main filter.
Everywhere where KHP is used, it is first completely reset, thus making
it unnecessary to keep it as a class member.
This saves 2.3KB RAM.
Stack sizes don't need changing, since there is already a function
Ekf::predictCovariance(), which needs around 3KB of stack and is called
close to where the fuse* functions are called.
All the decision for a sensor are made within a specific function for that sensor and when there is data to process at the fusion time horizon.
Information and warning messages are improved.
Wait until enough height has been gained to be clear of ground based magnetic anomalies. Failure to do so can result in incorrect earth field initialisation.
Convert uncertainty in initial rotate vector into quaternion covariances using symbolic toolbox derived expressions.
Enable setting of initial angle uncertainty via a parameter
Convert quaternion covariances into an angular alignment variance vector and discard the z component so that yaw uncertainty does not affect the result.
Replace the delayed time feedback mechanism used by the translational states with a direct feedback method.
Time constants for velocity and position convergence can be separately adjusted with tunable parameters
The method is more computationally more expensive because it requires modification of the output buffer history but is acceptable because it only requires 6 FLOP per buffer index for a total of 30*6 = 180 FLOP
The method was not applied to the attitude states because the quaternion operations required at each buffer index would have been computationally prohibitive.
Combines the forced symmetry, variance limiting and zeroing of covariances for unwanted states in the one function.
This ensures a consistent correction is applied after every covariance prediction or correction.
With the EKF, the average update rate is more important than the instantaneous value as it affects tuning. This patch ensures that the EKF prediction cycle will be performed early if the previous one was late in an attempt to maintain the target update rate.
Paul Riseborough