fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 04:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Add Emergency yaw recovery using EKF-GSF estimator (#766)

Browse Source 
* EKF: Use common rate vector calculation for offset corrections

* EKF: Remove duplicate matrix entry calculations

* EKF: Create a EKF-GSF yaw estimator class

* EKF: add emergency yaw reset functionality

* EKF: remove un-used function

* EKF: Ensure required constants are defined for all builds

* EKF: Fix CI build error

* Revert "EKF: remove un-used function"

This reverts commit 93005309c7f3794414ad99c86218b3062e00bbd3.

* EKF: Replace in-lined Tait-Bryan 312 conversions with function call

Also remove unnecessary operations

* EKF: Remove unnecessary update of external vision rotation matrix

* EKF: Use const

* EKF: use const

* EKF: don't use class variable as a temporary variable

* EKF: update comments

* EKF: Improve efficiency of yaw reset

Use conversion from rotation matrix to Euler angles instead of quaternion to euler angles.

* EKF: use const

* EKF: remove un-used struct element

* EKF: more descriptive function name

* EKF: use existing matrix row operator

* EKF: remove unnecessary rotation matrix update

* EKF: Use square matrix type

* EKF: Improve protection for bad innovation covariance

* EKF: Use matrix library operations

* EKF: Replace memcpy with better alternative

memcpy bypasses compiler sanity checks and is unnecessary in this instance.

* EKF: Split EKF-GSF yaw reset function

Adds a common function to support yaw reset that can be used elsewhere.

* EKF: Use common function for quaternion state and covariance yaw reset

* EKF: Replace inlined matrix operation

* EKF: Use const

* EKF: Change accessor function name

* EKF: Use const

* EKF: Don't create unnecessary duplicate variable locations

* EKF: Remove duplicate covariance innovation inverse

* EKF: Don't create unnecessary duplicate variable locations

* EKF: Rely on geo library to provide gravity

* EKF: Improve protection from bad updates

* EKF: Reduce effect of vibration on yaw estimator AHRS

* EKF: Improve yaw estimator AHRS accuracy during manoeuvre transients
This commit is contained in:
Paul Riseborough
2020-03-05 21:50:52 +11:00
committed by GitHub
parent 8ce285cdfa
commit 4669aa6312
10 changed files with 1129 additions and 231 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.h
+11
View File
@@ -46,6 +46,7 @@
#include "RingBuffer.h"
#include "AlphaFilter.hpp"
#include "imu_down_sampler.hpp"
#include "EKFGSF_yaw.h"
#include <geo/geo.h>
#include <matrix/math.hpp>
@@ -414,6 +415,13 @@ public:
static constexpr unsigned FILTER_UPDATE_PERIOD_MS{8}; // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
static constexpr float FILTER_UPDATE_PERIOD_S{FILTER_UPDATE_PERIOD_MS * 0.001f};
// request the EKF reset the yaw to the estimate from the internal EKF-GSF filter
// argment should be incremented only when a new reset is required
virtual void requestEmergencyNavReset(uint8_t counter) = 0;
// get ekf-gsf debug data
virtual bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) = 0;
protected:
parameters _params; // filter parameters
@@ -538,6 +546,9 @@ protected:
RingBuffer<dragSample> _drag_buffer;
RingBuffer<auxVelSample> _auxvel_buffer;
// yaw estimator instance
EKFGSF_yaw yawEstimator;
// observation buffer final allocation failed
bool _gps_buffer_fail{false};
bool _mag_buffer_fail{false};