Convert uncertainty in initial rotate vector into quaternion covariances using symbolic toolbox derived expressions.
Enable setting of initial angle uncertainty via a parameter
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.
Noise variance for stationary states was being overwritten by the covariance prediction operations for the kinematic states
Unwanted states could end up with non-zero covariance values
The forced symmetry operation was being applied too late to be effective
Parameters needed to be changed when covariance prediction error was corrected.
The magnetic earth and body field process noise now use separate parameters to help with tuning
Turn off the process noise if they grow too large and use the hard variance limit as a backup. This is numerically more stable than relying on a hard variance limit which does not deal with the covariances.
Use of the power function to square a number can be very inefficient depending on processor and compiler. Replace with an inlined function that multiplies the number by itself. This has not been put into the maths library because the use of the sq() function is peculiar to the autocode generated for the EKF and an inlined function was desired.
Set conservative defaults as a baseline for tuning
Add a missing parameter for magnetometer observation noise.
Correct error in definition of magnetic heading observations noise (previous parameter defined the variance directly, not the noise).
Scale vertical GPS noise to allow for average EPV to EPH ratio.
Update documentation and display names for consistency.
Carl Olsson
Paul Riseborough
Roman Bapst
bugobliterator
Roman