If EV height selected ensure switch to correct height mode as soon as EV data is received
The 0 height datum is not at the initialisation position, so the height state needs to be reset to the measurement on startup
Setting the velocity tracking tine constant to a negative number causes the output predictor to use a different method of correcting the velocity which provides a velocity output that is kinematically consistent with the position output.
This may improve height controller performance under some circumstances
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.
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
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)
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
Paul Riseborough
Carl Olsson
waltjohnson
Nic
devbharat
Beat Küng
Julian Oes