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.
This bug was in the derivation and resulted from use of a tan instead of an atan operator. The derivations have been reworked and the updated auto-code has been imported as part of this patch.
The atan function is now being used correctly instead of the tan function. This fixes problems with large heading errors or declination values.
The simple heading fusion has been decoupled from the magnetic field measurements. This enables external yaw measurements to be used in the future.
The position reset was not being compensated for velocity and measurement delay
The height was being reset with the position. It has been moved into a separate reset function
The maximum accepted GPS delay of 100msec was inadequate
The states was being incorrectly reset to the GPS position and Baro height on initial alignment.
The position reset was not being compensated for velocity and measurement delay
The height was being reset with the position. It has been moved into a separate reset function
The maximum accepted GPS delay of 100msec was inadequate
The states was being incorrectly reset to the GPS position and Baro height on initial alignment.
When fusing 3-axis magnetometer data without absolute external aiding leg GPS), the declination is ultimately unobservable and the declination of the field states and the vehicle heading will slowly drift over time. To prevent this we need to fuse in a declination to constraint the NE earth field estimates.
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.
A larger position uncertainty is required when flying without GPS to reduce tilt attitude estimation errors caused by vehicle manoeuvring. This needs to be tuneable to allow optimisation for different use cases (e.g. outdoor vs indoor).
Fix bug where noise parameters were not being squared when calculating the observation variance.
Use GPS reported speed accuracy to set velocity observation noise and use the parameter to set the minimum.
bugobliterator
Paul Riseborough
Roman Bapst
Paul Riseborough
CarlOlsson
Lorenz Meier