EKF: Add Emergency yaw recovery using EKF-GSF estimator #754
Conversation
Squashed commits: [86e367a] EKF: fix wrong variable type [10fa403] EKF: Prevent reset to EKF-GSF yaw caused by flight without GPS velocity fusion [c698557] EKF: declare variables const [a8fc45a] EKF: fix typos [0d804ed] EKF: remove unused struct member [e7c2ec1] EKF: reset to the EKF-GSF yaw after 1 second of rejected horizontal GPS velocity measurements [4f1a33a] EKF: Non-functional changes to improve clarity of EKF-GSF tilt alignment [50f8729] EKF: EKF-GSF replace inlined code with call to exisiting function [b2c379d] EKF: EKF-GSF rework innovation compression scale factor Don't apply compression scale factor to the innovations that are used to calculate weightings. [6b905a6] EKF: EKF-GSF use const type and update comments [0ead696] EKF: EKF-GSF protect velocity fusion from measurement spikes Also fix order of calculation for innovation variance subsequently used by the weighting calculation. It should be before the covariance is updated by the measurement [012caac] EKF: fix comments [dd8bb26] EKF: EKF-GSF improve efficiency of handling badly conditioned Kalman gain calculation [910e3da] EKF: Add prototype EKF-GSF yaw estimator with emergency yaw reset
|
@dagar I was thinking it may make sense to have this in a separate class, but still run it from inside the main EKF to maintain access to the time aligned IMU and GPS data and to enable reset to use data at a common time horizon. |
|
Data from five flights with a 100 degree magnetic yaw error. Each flight started in Position mode, The flyaway started immediately after takeoff and the EKF reset successfully each time and without any subsequent commander failsafe activation. The vehicle could be flown back in position mode each time. Logs here: https://logs.px4.io/plot_app?log=1d7a16f5-6476-4c6a-a55c-d0336ab01762 Sample plot from first flight showing filter behaviour and EKF yaw resetting to EKF-GSF composite shortly after takeoff.
|
As the name can be ambiguous, it gets renamed "uncorrelateQuatFromOtherStates". Also replace the loops storing the values and reapplying them by simply zeroing two slices (this also saves 130B of flash).
|
Here's the callgrind output from a SITL test 'make px4_sitl gazebo_iris_callgrind' (graphed using KCachegrind). The EKF-GSF makes up about 16% of total EKF output.
|
Remove access to preliminary data only used for initial 3-state algorithm development in Matlab. Add access to composite yaw variance.
Changes arising from peer review.
Change arising from peer review
Change function name to better reflect intended usage and effect. Improve documentation.
Uses direct application of gyro to rotation matrix in AHRs calculations. The elimination of unnecessary conversions between quaternions and other attitude descriptions has reduced computational load.
priseborough
force-pushed
the
pr-yawEKFGSF
branch
from
b255b40 to
f8bdd16
Compare
Removes out of data references to use of quaternions.
|
Here's the callgrind output from a SITL test 'make px4_sitl gazebo_iris_callgrind' (graphed using KCachegrind) after elimination of the quaternion operations. With 8 filter banks running, the EKF-GSF now makes up 11.2% of total EKF CPU usage, down from 15.8%.
With 3 filter banks, this drops to 5.3% of total EKF CPU load
|
Use pre-computed inverse. Don't perform unnecessary rotation matrix to yaw angle conversion.
Only copy required rows.
|
Flight test replay at commit 44d34e6 This period of flight shows a vertical takeoff with a low speed horizontal position change at up to 2.5 m/s initiated at 95 seconds.
|
|
Replay of the same log comparing N_MODELS_EKFGSF set to 8 vs 5 vs 3 shows the Gaussian sum filter (GSF) method of combining the individual EKF states to be more effective than originally anticipated. Similar results have been achieved wth replay of all logs to date so we may be able to reduce the number of filters to 3. The caveat is how well this insensitivity to reduction in the value of N_MODELS_EKFGSF holds with noisier GPS data. This log has a healthy main EKF so the EKF yaw (in black) provides a reference for checking the convergence of the composite yaw estimate from the GSF (in red). The vehicle performs a vertical takeoff and commences a slow 2.5 m/s position change at 95 seconds. The residual horizontal movement of the vehicle during the vertical climb is sufficient to enable the GSF to estimate the yaw correctly, and the GSF's reported yaw variance falls to a level that would enable a reset before the horizontal position change manoeuvre commences. N_MODELS_EKFGSF = 8 N_MODELS_EKFGSF = 5 N_MODELS_EKFGSF = 3 |
|
I have refactored this to move the estimator into it's own class so am now closing this PR and replacing it with #764. |
Replaced with #764
This is functionally the same as #751 and any changes identified during review and testing of #751 will be cherry-picked into this branch.
Testing used https://github.com/priseborough/Firmware/tree/pr-yawEKFGSF PX4/Firmware version.
Flight testing of this version to follow, but here is replay output using https://logs.px4.io/plot_app?log=d1e5d8e4-5c44-4350-b355-ee3acb2503b8 where a navigation failure was induced by taking off with the declination set to an error of 100 degrees. This can be achieved by setting EKF2_MAG_DECL to a large value at least 90 degrees away from the declination at the test location and EKF2_DECL_TYPE = 0 to force the EKF to use the parameter value.
If you do testing make sure you have a non-position control mode like altitude or manual/stabilized available.