[ekf] controlMagFusion refactor and mag field strength check (#662)

* ekf_control: Inhibit mag fusion when field magnitude is large
Move mag inhibition check in separate function

* ekf_control: pull out of functionalities out of controlMagFusion
- yaw abd mag bias observability checks
- mag 3D conditions
- load mag covariances
- set and clear mag control modes

* ekf_control: refactor mag heading/3D start/stop.
Move mag declination, mag 3d and mag heading fusion out of the main function

* ekf_control: extract mag yaw reset and mag declination fusion requirements

* ekf_control: use WMM in isStronMagneticField for mag fusion inhibition
- Correct units of WMM strength table

* ekf_control: extract mag_state_only functionality of AUTOFW (VTOL custom)
Also split inAirYawReset from onGroundYawReset

* ekf_control: extract mag automatic selection
- transform if-else into switch-case for parameter fusion type selection

* ekf_control: extract run3DMagAndDeclFusion, reorganize functions, fix
flag naming in Test script

* ekf_control: do not run mag fusion if tilt is not aligned.
Reset some variables on ground even if mag fusion is not running yet. It
could be that it runs later so we need to make sure that those variables
are properly set.

* ekf_control: move controlMagFusion and related functions to mag_control.cpp

* ekf control: check for validity of mag strength from WMM and falls back
to average earth mag field with larger gate if not valid

* ekf control: remove evyaw check for mag inhibition

* ekf control: change nested ternary operator into if-else if

* Ekf: create AlphaFilter template class for simple low-pass filtering
0.1/0.9 type low-pass filters are commonly used to smooth data, this
class is meant to abstract the computation of this filter

* ekf control: reset heading using mag_lpf data to avoid resetting on an outlier
fixes ecl issue #525

* ekf control: replace mag_states_only flag with mag_field_disturbed and
add parameter to enable or disable mag field strength check

* ekf control: remove AUTOFW mag fusion type as not needed This was implemented for VTOL but did not solve the problem and should not be used anymore

* ekf control: use start/stop mag functions everywhere instead of setting the flag

* ekf control: Run mag fusion depending on yaw_align instead of tilt_align
as there is no reason to fuse mag when the ekf isn't aligned

* AlphaFilter: add test for float and Vector3f

EKF/ekf.h

@@ -200,7 +200,7 @@ public:
 	// check if the EKF is dead reckoning horizontal velocity using inertial data only
 	void update_deadreckoning_status();
 
-	bool isTerrainEstimateValid() override;
+	bool isTerrainEstimateValid() const override;
 
 	void updateTerrainValidity();
 
@@ -430,7 +430,7 @@ private:
 	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
 	uint32_t _ev_counter{0};		///< number of external vision samples read during initialisation
 	uint64_t _time_last_mag{0};		///< measurement time of last magnetomter sample (uSec)
-	Vector3f _mag_filt_state;		///< filtered magnetometer measurement (Gauss)
+	AlphaFilterVector3f _mag_lpf;		///< filtered magnetometer measurement (Gauss)
 	Vector3f _delVel_sum;			///< summed delta velocity (m/sec)
 	float _hgt_sensor_offset{0.0f};		///< set as necessary if desired to maintain the same height after a height reset (m)
 	float _baro_hgt_offset{0.0f};		///< baro height reading at the local NED origin (m)
@@ -439,7 +439,7 @@ private:
 	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
 	bool _flt_mag_align_converging{false};	///< true when the in-flight mag field post alignment convergence is being performd
 	uint64_t _flt_mag_align_start_time{0};	///< time that inflight magnetic field alignment started (uSec)
-	uint64_t _time_last_movement{0};	///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
+	uint64_t _time_last_mov_3d_mag_suitable{0};	///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
 	float _saved_mag_bf_variance[4] {};	///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
 	float _saved_mag_ef_covmat[2][2] {};    ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
 	bool _velpos_reset_request{false};	///< true when a large yaw error has been fixed and a velocity and position state reset is required
@@ -561,7 +561,7 @@ private:
 
 	// reset the heading and magnetic field states using the declination and magnetometer/external vision measurements
 	// return true if successful
-	bool resetMagHeading(Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer=true);
+	bool resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer=true);
 
 	// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
 	// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle.
@@ -621,6 +621,38 @@ private:
 
 	// control fusion of magnetometer observations
 	void controlMagFusion();
+	void updateMagFilter();
+
+	bool canRunMagFusion() const;
+
+	void checkHaglYawResetReq();
+	float getTerrainVPos() const;
+
+	void runOnGroundYawReset();
+	bool isYawResetAuthorized() const;
+	bool canResetMagHeading() const;
+	void runInAirYawReset();
+	bool canRealignYawUsingGps() const;
+	void runVelPosReset();
+
+	void selectMagAuto();
+	void check3DMagFusionSuitability();
+	void checkYawAngleObservability();
+	void checkMagBiasObservability();
+	bool isYawAngleObservable() const;
+	bool isMagBiasObservable() const;
+	bool canUse3DMagFusion() const;
+
+	void checkMagDeclRequired();
+	void checkMagInhibition();
+	bool shouldInhibitMag() const;
+	void checkMagFieldStrength();
+	bool isStrongMagneticDisturbance() const;
+	bool isMeasuredMatchingGpsMagStrength() const;
+	bool isMeasuredMatchingAverageMagStrength() const;
+	static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
+	void runMagAndMagDeclFusions();
+	void run3DMagAndDeclFusions();
 
 	// control fusion of range finder observations
 	void controlRangeFinderFusion();
@@ -674,6 +706,12 @@ private:
 	// set control flags to use external vision height
 	void setControlEVHeight();
 
+	void stopMagFusion();
+	void stopMag3DFusion();
+	void stopMagHdgFusion();
+	void startMagHdgFusion();
+	void startMag3DFusion();
+
 	// zero the specified range of rows in the state covariance matrix
 	void zeroRows(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
 
@@ -697,7 +735,7 @@ private:
 	void initialiseQuatCovariances(Vector3f &rot_vec_var);
 
 	// perform a limited reset of the magnetic field state covariances
-	void resetMagCovariance();
+	void resetMagRelatedCovariances();
 
 	// perform a limited reset of the wind state covariances
 	void resetWindCovariance();
@@ -714,8 +752,11 @@ private:
 	// Argument is additional yaw variance in rad**2
 	void increaseQuatYawErrVariance(float yaw_variance);
 
-	// save mag field state covariance data for re-use
-	void save_mag_cov_data();
+	// load and save mag field state covariance data for re-use
+	void loadMagCovData();
+	void saveMagCovData();
+	void clearMagCov();
+	void zeroMagCov();
 
 	// uncorrelate quaternion states from other states
 	void uncorrelateQuatStates();