* Revert "EKF: Release flow speed limit with altitude gained"
This reverts commit e70206f74b.
* Revert "fix code style"
This reverts commit 76bf70121c.
* Revert "Reverse the linked list of data_validator_group and maintain a first node"
This reverts commit 32482e7644.
Gate size class variables should not be initialised to zero, because it will cause a /0 error if fuseVelPosHeight() is called before they are set to their respective parameter values.
When GPS use is gained whilst flying using optical flow data, the sudden release of the speed limit is unannounced to the operator and can cause unexpected acceleration.
This patch releases the speed limit as height is gained, but does not reduce it when the vehicle descends, unless GPS use is lost.
This enables the EKF to use an additional NE velocity measurement. This can be used to improve position hold stability when landing using a beacon system for positioning by fusing the beacon velocity estimates.
Ensures that a complete reset of velocity and position states will always be performed if yaw has had to be reset using GPS velocity.
Ensures that the yaw_align status cannot be set to false once the filter has aligned.
Fuse external vision data using a relative position odometry method when GPS data is also being used and enable both GPOS and EV data to be fused on the same time step.
the primary height source
- moved height control into single function in order to decide which sensor
should be used for estimating height
- under certain conditions allow to use the range finder to estimate height
even if it's not the primary height source
- fixed a bug where the delta time for the baro offset calculation was always
zero
- use methods to set height control flags to reduce code duplication and
to prevent bugs
Signed-off-by: Roman <bapstroman@gmail.com>
Use horizontal acceleration to check if yaw is observable independent of the magnetometer.
Use rotation about the vertical to check if mag raises are observable.
If neither yaw of mag biases are observable, save the magnetic field variances and switch to magnetic yaw fusion.
Use the last learned declination when using magnetic yaw fusion so that the yaw reference remains consistent.
When yaw or biases become observable, reinstate the saved variances and switch back to 3D mag fusion.
Use vertical velocity and position innovation failure to detect bad accelerometer data caused by clipping or aliasing which can cause large vertical acceleration errors and loss of height estimation. When bad accel data is detected:
1) Inhibit accelerometer bias learning
2) Force fusion of vertical velocity and height data
3) Increase accelerometer process noise
The previous practice of relying on the off-diagonals being zero caused problems with conditioning of the magnetometer fusion on one flight. By storing the variances when the learning inhibit becomes active and ensuring that the rows and columns in the covariance matrix for the inhibited states are always zero, the observed numerical conditioning error has been eliminated for replay of the problem flight log .
Make the target EKF rate an integer multiple of the IMU rate. This slightly increases the average prediction time step for the EKF from just over 10msec to 12msec, but the variation reduces significantly which makes filter tuning more deterministic.
Improve the algorithm used to adjust the collection time criteria to reduce jitter in the correction.
Daniel Agar
Carl Olsson
Paul Riseborough
Paul Riseborough
ChristophTobler
ChristophTobler
Roman