Merge pull request #1913 from Zefz/ekf-fixes

Fix EKF Attitude Position Estimator bugs

4 files changed, 143 insertions, 154 deletions

diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
index ec9efe8ee..4d5e56a96 100644
--- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
@@ -174,7 +174,6 @@ private:
 
     struct map_projection_reference_s   _pos_ref;
 
-    float                       _baro_ref;      /**< barometer reference altitude */
     float                       _baro_ref_offset;   /**< offset between initial baro reference and GPS init baro altitude */
     float                       _baro_gps_offset;   /**< offset between baro altitude (at GPS init time) and GPS altitude */
     hrt_abstime                 _last_debug_print = 0;
@@ -194,6 +193,7 @@ private:
     bool            _gpsIsGood;               ///< True if the current GPS fix is good enough for us to use
     uint64_t        _previousGPSTimestamp;    ///< Timestamp of last good GPS fix we have received
     bool            _baro_init;
+    float           _baroAltRef;
     bool            _gps_initialized;
     hrt_abstime     _filter_start_time;
     hrt_abstime     _last_sensor_timestamp;
@@ -208,7 +208,6 @@ private:
     bool            _ekf_logging;       ///< log EKF state
     unsigned        _debug;             ///< debug level - default 0
 
-    bool            _newDataGps;
     bool            _newHgtData;
     bool            _newAdsData;
     bool            _newDataMag;
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 3bb395a87..6ca10e56a 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -132,72 +132,71 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_wind_pub(-1),
 
 	_att({}),
-     _gyro({}),
-     _accel({}),
-     _mag({}),
-     _airspeed({}),
-     _baro({}),
-     _vstatus({}),
-     _global_pos({}),
-     _local_pos({}),
-     _gps({}),
-     _wind({}),
-     _distance {},
-     _landDetector {},
-     _armed {},
-
-     _gyro_offsets({}),
-     _accel_offsets({}),
-     _mag_offsets({}),
-
-     _sensor_combined {},
-
-     _pos_ref {},
-     _baro_ref(0.0f),
-     _baro_ref_offset(0.0f),
-     _baro_gps_offset(0.0f),
-
-     /* performance counters */
-     _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
-     _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
-     _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
-     _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
-     _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
-     _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
-     _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
-     _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
-
-     /* states */
-     _gps_alt_filt(0.0f),
-     _baro_alt_filt(0.0f),
-     _covariancePredictionDt(0.0f),
-     _gpsIsGood(false),
-     _previousGPSTimestamp(0),
-     _baro_init(false),
-     _gps_initialized(false),
-     _filter_start_time(0),
-     _last_sensor_timestamp(0),
-     _last_run(0),
-     _distance_last_valid(0),
-     _gyro_valid(false),
-     _accel_valid(false),
-     _mag_valid(false),
-     _gyro_main(0),
-     _accel_main(0),
-     _mag_main(0),
-     _ekf_logging(true),
-     _debug(0),
-
-     _newDataGps(false),
-     _newHgtData(false),
-     _newAdsData(false),
-     _newDataMag(false),
-     _newRangeData(false),
-
-     _mavlink_fd(-1),
-     _parameters {},
-     _parameter_handles {},
-     _ekf(nullptr)
+    _gyro({}),
+    _accel({}),
+    _mag({}),
+    _airspeed({}),
+    _baro({}),
+    _vstatus({}),
+    _global_pos({}),
+    _local_pos({}),
+    _gps({}),
+    _wind({}),
+    _distance {},
+    _landDetector {},
+    _armed {},
+
+    _gyro_offsets({}),
+    _accel_offsets({}),
+    _mag_offsets({}),
+
+    _sensor_combined {},
+
+    _pos_ref{},
+    _baro_ref_offset(0.0f),
+    _baro_gps_offset(0.0f),
+
+    /* performance counters */
+    _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
+    _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
+    _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
+    _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
+    _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
+    _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
+    _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
+    _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
+
+    /* states */
+    _gps_alt_filt(0.0f),
+    _baro_alt_filt(0.0f),
+    _covariancePredictionDt(0.0f),
+    _gpsIsGood(false),
+    _previousGPSTimestamp(0),
+    _baro_init(false),
+    _baroAltRef(0.0f),
+    _gps_initialized(false),
+    _filter_start_time(0),
+    _last_sensor_timestamp(0),
+    _last_run(0),
+    _distance_last_valid(0),
+    _gyro_valid(false),
+    _accel_valid(false),
+    _mag_valid(false),
+    _gyro_main(0),
+    _accel_main(0),
+    _mag_main(0),
+    _ekf_logging(true),
+    _debug(0),
+
+    _newHgtData(false),
+    _newAdsData(false),
+    _newDataMag(false),
+    _newRangeData(false),
+
+    _mavlink_fd(-1),
+    _parameters {},
+    _parameter_handles {},
+    _ekf(nullptr)
 {
 	_last_run = hrt_absolute_time();
 
@@ -636,24 +635,26 @@ void AttitudePositionEstimatorEKF::task_main()
 //				}
 
 				/* Initialize the filter first */
-				if (!_gps_initialized && _gpsIsGood) {
-					initializeGPS();
-
-				} else if (!_ekf->statesInitialised) {
+				if (!_ekf->statesInitialised) {
 					// North, East Down position (m)
 					float initVelNED[3] = {0.0f, 0.0f, 0.0f};
 
 					_ekf->posNE[0] = 0.0f;
 					_ekf->posNE[1] = 0.0f;
 
-					_local_pos.ref_alt = _baro_ref;
+					_local_pos.ref_alt = 0.0f;
 					_baro_ref_offset = 0.0f;
 					_baro_gps_offset = 0.0f;
 					_baro_alt_filt = _baro.altitude;
 
 					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
 
-				} else if (_ekf->statesInitialised) {
+				} else {
+
+					if (!_gps_initialized && _gpsIsGood) {
+						initializeGPS();
+						continue;
+					}
 
 					// Check if on ground - status is used by covariance prediction
 					_ekf->setOnGround(_landDetector.landed);
@@ -668,7 +669,7 @@ void AttitudePositionEstimatorEKF::task_main()
 					}
 
 					//Run EKF data fusion steps
-					updateSensorFusion(_newDataGps, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
+					updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
 
 					//Publish attitude estimations
 					publishAttitude();
@@ -717,7 +718,7 @@ void AttitudePositionEstimatorEKF::initializeGPS()
 	_baro_alt_filt = _baro.altitude;
 
 	_ekf->baroHgt = _baro.altitude;
-	_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
+	_ekf->hgtMea = _ekf->baroHgt;
 
 	// Set up position variables correctly
 	_ekf->GPSstatus = _gps.fix_type;
@@ -810,7 +811,7 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
 	_local_pos.y = _ekf->states[8];
 
 	// XXX need to announce change of Z reference somehow elegantly
-	_local_pos.z = _ekf->states[9] - _baro_ref_offset;
+	_local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef;
 
 	_local_pos.vx = _ekf->states[4];
 	_local_pos.vy = _ekf->states[5];
@@ -858,7 +859,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 	}
 
 	/* local pos alt is negative, change sign and add alt offsets */
-	_global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset;
+	_global_pos.alt = (-_local_pos.z) - _baro_gps_offset;
 
 	if (_local_pos.v_z_valid) {
 		_global_pos.vel_d = _local_pos.vz;
@@ -908,8 +909,7 @@ void AttitudePositionEstimatorEKF::publishWindEstimate()
 }
 
 void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag,
-		const bool fuseRangeSensor,
-		const bool fuseBaro, const bool fuseAirSpeed)
+		const bool fuseRangeSensor, const bool fuseBaro, const bool fuseAirSpeed)
 {
 	// Run the strapdown INS equations every IMU update
 	_ekf->UpdateStrapdownEquationsNED();
@@ -978,7 +978,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 
 	if (fuseBaro) {
 		// Could use a blend of GPS and baro alt data if desired
-		_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
+		_ekf->hgtMea = _ekf->baroHgt;
 		_ekf->fuseHgtData = true;
 
 		// recall states stored at time of measurement after adjusting for delays
@@ -1071,7 +1071,7 @@ void AttitudePositionEstimatorEKF::print_status()
 
 	printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
 	printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f));
-	printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset,
+	printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset,
 	       (double)_baro_gps_offset);
 	printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y,
 	       (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
@@ -1268,10 +1268,10 @@ void AttitudePositionEstimatorEKF::pollData()
 	}
 
 
-	orb_check(_gps_sub, &_newDataGps);
-
-	if (_newDataGps) {
+	bool gps_update;
+	orb_check(_gps_sub, &gps_update);
 
+	if (gps_update) {
 		orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
 		perf_count(_perf_gps);
 
@@ -1324,10 +1324,7 @@ void AttitudePositionEstimatorEKF::pollData()
 			if (_gps_initialized) {
 
 				//Convert from global frame to local frame
-				float posNED[3] = {0.0f, 0.0f, 0.0f};
-				_ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
-				_ekf->posNE[0] = posNED[0];
-				_ekf->posNE[1] = posNED[1];
+				map_projection_project(&_pos_ref, (_gps.lat / 1.0e7), (_gps.lon / 1.0e7), &_ekf->posNE[0], &_ekf->posNE[1]);
 
 				if (dtLastGoodGPS > POS_RESET_THRESHOLD) {
 					_ekf->ResetPosition();
@@ -1353,9 +1350,6 @@ void AttitudePositionEstimatorEKF::pollData()
 
 			_previousGPSTimestamp = _gps.timestamp_position;
 
-		} else {
-			//Too poor GPS fix to use
-			_newDataGps = false;
 		}
 	}
 
@@ -1406,21 +1400,17 @@ void AttitudePositionEstimatorEKF::pollData()
 		}
 
 		baro_last = _baro.timestamp;
+		if(!_baro_init) {
+			_baro_init = true;
+			_baroAltRef = _baro.altitude;
+		}
 
 		_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
 
 		_ekf->baroHgt = _baro.altitude;
 		_baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
 
-		if (!_baro_init) {
-			_baro_ref = _baro.altitude;
-			_baro_init = true;
-			warnx("ALT REF INIT");
-		}
-
 		perf_count(_perf_baro);
-
-		_newHgtData = true;
 	}
 
 	//Update Magnetometer
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index 5c01286e3..c313e83af 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -2329,15 +2329,20 @@ void AttPosEKF::zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATE
 // Store states in a history array along with time stamp
 void AttPosEKF::StoreStates(uint64_t timestamp_ms)
 {
-    for (size_t i=0; i<EKF_STATE_ESTIMATES; i++)
+    for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
         storedStates[i][storeIndex] = states[i];
+    }
+
     storedOmega[0][storeIndex] = angRate.x;
     storedOmega[1][storeIndex] = angRate.y;
     storedOmega[2][storeIndex] = angRate.z;
     statetimeStamp[storeIndex] = timestamp_ms;
+
+    // increment to next storage index
     storeIndex++;
-    if (storeIndex == EKF_DATA_BUFFER_SIZE)
+    if (storeIndex >= EKF_DATA_BUFFER_SIZE) {
         storeIndex = 0;
+    }
 }
 
 void AttPosEKF::ResetStoredStates()
@@ -2350,10 +2355,8 @@ void AttPosEKF::ResetStoredStates()
     // reset store index to first
     storeIndex = 0;
 
-    statetimeStamp[storeIndex] = millis();
-
-    // increment to next storage index
-    storeIndex++;
+    //Reset stored state to current state
+    StoreStates(millis());
 }
 
 // Output the state vector stored at the time that best matches that specified by msec
@@ -2513,27 +2516,6 @@ void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4])
     y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
 }
 
-void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
-{
-    velNED[0] = gpsGndSpd*cosf(gpsCourse);
-    velNED[1] = gpsGndSpd*sinf(gpsCourse);
-    velNED[2] = gpsVelD;
-}
-
-void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference)
-{
-    posNED[0] = earthRadius * (lat - latReference);
-    posNED[1] = earthRadius * cos(latReference) * (lon - lonReference);
-    posNED[2] = -(hgt - hgtReference);
-}
-
-void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
-{
-    lat = latRef + (double)posNED[0] * earthRadiusInv;
-    lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef);
-    hgt = hgtRef - posNED[2];
-}
-
 void AttPosEKF::setOnGround(const bool isLanded)
 {
     _onGround = isLanded;
@@ -2592,25 +2574,40 @@ void AttPosEKF::CovarianceInit()
     P[1][1]   = 0.25f * sq(1.0f*deg2rad);
     P[2][2]   = 0.25f * sq(1.0f*deg2rad);
     P[3][3]   = 0.25f * sq(10.0f*deg2rad);
+
+    //velocities
     P[4][4]   = sq(0.7f);
     P[5][5]   = P[4][4];
     P[6][6]   = sq(0.7f);
+
+    //positions
     P[7][7]   = sq(15.0f);
     P[8][8]   = P[7][7];
     P[9][9]   = sq(5.0f);
+
+    //delta angle biases
     P[10][10] = sq(0.1f*deg2rad*dtIMU);
     P[11][11] = P[10][10];
     P[12][12] = P[10][10];
+
+    //Z delta velocity bias
     P[13][13] = sq(0.2f*dtIMU);
-    P[14][14] = sq(0.0f);
+
+    //Wind velocities
+    P[14][14] = 0.0f;
     P[15][15]  = P[14][14];
+
+    //Earth magnetic field
     P[16][16] = sq(0.02f);
     P[17][17] = P[16][16];
     P[18][18] = P[16][16];
+
+    //Body magnetic field
     P[19][19] = sq(0.02f);
     P[20][20] = P[19][19];
     P[21][21] = P[19][19];
 
+    //Optical flow
     fScaleFactorVar = 0.001f; // focal length scale factor variance
     Popt[0][0] = 0.001f;
 }
@@ -2628,9 +2625,11 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val)
         ret = val;
     }
 
+#if 0
     if (!isfinite(val)) {
-        //ekf_debug("constrain: non-finite!");
+        ekf_debug("constrain: non-finite!");
     }
+#endif
 
     return ret;
 }
@@ -2863,8 +2862,12 @@ void AttPosEKF::ResetPosition(void)
         for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
             storedStates[7][i] = states[7];
             storedStates[8][i] = states[8];
-        }        
+        }
     }
+
+    //reset position covariance
+    P[7][7]   = sq(15.0f);
+    P[8][8]   = P[7][7];    
 }
 
 void AttPosEKF::ResetHeight(void)
@@ -2876,6 +2879,10 @@ void AttPosEKF::ResetHeight(void)
     for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
         storedStates[9][i] = states[9];
     }    
+
+    //reset altitude covariance
+    P[9][9] = sq(5.0f);
+    P[6][6] = sq(0.7f);
 }
 
 void AttPosEKF::ResetVelocity(void)
@@ -2884,7 +2891,8 @@ void AttPosEKF::ResetVelocity(void)
         states[4] = 0.0f;
         states[5] = 0.0f;
         states[6] = 0.0f;
-    } else if (GPSstatus >= GPS_FIX_3D) {
+    } 
+    else if (GPSstatus >= GPS_FIX_3D) {
         //Do not use Z velocity, we trust the Barometer history more
 
         states[4]  = velNED[0]; // north velocity from last reading
@@ -2894,8 +2902,12 @@ void AttPosEKF::ResetVelocity(void)
         for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
             storedStates[4][i] = states[4];
             storedStates[5][i] = states[5];
-        }                
+        }          
     }
+
+    //reset velocities covariance
+    P[4][4]   = sq(0.7f);
+    P[5][5]   = P[4][4]; 
 }
 
 bool AttPosEKF::StatesNaN() {
@@ -3012,10 +3024,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
         // Fill error report
         GetFilterState(&last_ekf_error);
 
-        ResetStoredStates();
         ResetVelocity();
         ResetPosition();
         ResetHeight();
+        ResetStoredStates();
 
         // Timeout cleared with this reset
         current_ekf_state.imuTimeout = false;
@@ -3029,10 +3041,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
         // Fill error report, but not setting error flag
         GetFilterState(&last_ekf_error);
 
-        ResetStoredStates();
         ResetVelocity();
         ResetPosition();
         ResetHeight();
+        ResetStoredStates();
 
         ret = 0;
     }
@@ -3202,10 +3214,10 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
     states[20] = magBias.y; // Magnetic Field Bias Y
     states[21] = magBias.z; // Magnetic Field Bias Z
 
-    ResetStoredStates();
     ResetVelocity();
     ResetPosition();
     ResetHeight();
+    ResetStoredStates();
 
     // initialise focal length scale factor estimator states
     flowStates[0] = 1.0f;
@@ -3217,7 +3229,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
 
     //Define Earth rotation vector in the NED navigation frame
     calcEarthRateNED(earthRateNED, latRef);
-
 }
 
 void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination)
@@ -3293,7 +3304,6 @@ void AttPosEKF::ZeroVariables()
     magstate.DCM.identity();
 
     memset(&current_ekf_state, 0, sizeof(current_ekf_state));
-
 }
 
 void AttPosEKF::GetFilterState(struct ekf_status_report *err)
@@ -3310,13 +3320,6 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err)
     current_ekf_state.useAirspeed = useAirspeed;
 
     memcpy(err, &current_ekf_state, sizeof(*err));
-
-    // err->velHealth = current_ekf_state.velHealth;
-    // err->posHealth = current_ekf_state.posHealth;
-    // err->hgtHealth = current_ekf_state.hgtHealth;
-    // err->velTimeout = current_ekf_state.velTimeout;
-    // err->posTimeout = current_ekf_state.posTimeout;
-    // err->hgtTimeout = current_ekf_state.hgtTimeout;
 }
 
 void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h
index c5517e38b..9b23f4df4 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h
@@ -288,7 +288,6 @@ public:
      * Recall the state vector.
      *
      * Recalls the vector stored at closest time to the one specified by msec
-     *FuseOptFlow
      * @return zero on success, integer indicating the number of invalid states on failure.
      *         Does only copy valid states, if the statesForFusion vector was initialized
      *         correctly by the caller, the result can be safely used, but is a mixture
@@ -307,12 +306,6 @@ public:
 
     static void quat2eul(float (&eul)[3], const float (&quat)[4]);
 
-    static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
-
-    static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
-
-    static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
-
     //static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
 
     static inline float sq(float valIn) {return valIn * valIn;}
@@ -362,8 +355,6 @@ public:
      */
     void ResetVelocity();
 
-    void ZeroVariables();
-
     void GetFilterState(struct ekf_status_report *state);
 
     void GetLastErrorState(struct ekf_status_report *last_error);
@@ -381,6 +372,12 @@ public:
     *   true if the vehicle moves like a Fixed Wing, false otherwise
     **/
     void setIsFixedWing(const bool fixedWing);
+    
+    /**
+     * @brief
+     *   Reset internal filter states and clear all variables to zero value
+     */
+    void ZeroVariables();
 
 protected:
 
@@ -409,7 +406,7 @@ protected:
     void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
 
     void ResetStoredStates();
-    
+
 private:
     bool _isFixedWing;               ///< True if the vehicle is a fixed-wing frame type
     bool _onGround;                  ///< boolean true when the flight vehicle is on the ground (not flying)