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.
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.
Beat Küng
Paul Riseborough