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.
Use IMU driver published delta angle integration time to determine when enough data has been accumulated.
Perform basic first order sculling corrections on delta velocity data
Comment steps to make make method clearer
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
in PX4 Firmware, Ekf::get_terrain_vert_pos depends on it. This is the
valgrind output:
==15439== Thread 14 ekf2:
==15439== Conditional jump or move depends on uninitialised value(s)
==15439== at 0x5610087: sqrtf (in /usr/lib64/libm-2.22.so)
==15439== by 0x4D1AF0: Ekf::get_terrain_vert_pos(float*) (terrain_estimator.cpp:135)
==15439== by 0x46641F: Ekf2::task_main() (ekf2_main.cpp:655)
==15439== by 0x42BF3D: entry_adapter(void*) (px4_posix_tasks.cpp:103)
==15439== by 0x4E3C609: start_thread (in /usr/lib64/libpthread-2.22.so)
==15439== by 0x5BF7A4C: clone (in /usr/lib64/libc-2.22.so)
The cross terms (covariances) are now reset before the variance variance is set.
The vertical velocity state variance following a reset without GPS has been reduced.
IMU corrections were being applied at the wrong time horizon to the EKF states
IMU downsampling rotates the delta velocities into the orientation produced by the last delta angle, but this wasn't consistent with the way the state prediction was using the delta velocity data.
Paul Riseborough
Daniel Agar
Beat Küng
Roman
Julian Oes