gps: fixed filter initialization bug around gps

- do not gps reference altitude to zero in case gps checks pass before the
filter initialized
- reset the filtered gps position and position derivative filters in case
we are in air or there is movement on the ground

Signed-off-by: RomanBapst <bapstroman@gmail.com>

EKF/ekf.cpp

@@ -93,6 +93,10 @@ void Ekf::reset()
 	_accel_magnitude_filt = 0.0f;
 	_ang_rate_magnitude_filt = 0.0f;
 	_prev_dvel_bias_var.zero();
+
+	_gps_alt_ref = 0.0f;
+
+	resetGpsDriftCheckFilters();
 }
 
 bool Ekf::update()
@@ -189,10 +193,6 @@ bool Ekf::initialiseFilter()
 		// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
 		setControlBaroHeight();
 
-		// reset variables that are shared with post alignment GPS checks
-		_gps_pos_deriv_filt(2) = 0.0f;
-		_gps_alt_ref = 0.0f;
-
 		if(!initialiseTilt()){
 			return false;
 		}

EKF/ekf.h

@@ -893,4 +893,5 @@ private:
 	// Returns true if the reset was successful
 	bool resetYawToEKFGSF();
 
+	void resetGpsDriftCheckFilters();
 };

EKF/ekf_helper.cpp

@@ -1866,3 +1866,9 @@ void Ekf::runYawEKFGSF()
 		yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc);
 	}
 }
+
+void Ekf::resetGpsDriftCheckFilters()
+{
+	_gps_velNE_filt.setZero();
+	_gps_pos_deriv_filt.setZero();
+}

EKF/gps_checks.cpp

@@ -194,10 +194,13 @@ bool Ekf::gps_is_good(const gps_message &gps)
 		_gps_check_fail_status.flags.hspeed = false;
 		_gps_drift_updated = false;
 
+		resetGpsDriftCheckFilters();
+
 	} else {
 		// This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation
 		_gps_drift_updated = true;
 
+		resetGpsDriftCheckFilters();
 	}
 
 	// save GPS fix for next time