EKF2: reset global position using variance

PX4 · Dec 3, 2024 · 8626019 · 8626019
1 parent 6b637f8
commit 8626019
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 6 deletions.
diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp
@@ -110,8 +110,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 
 				} else {
 					// Try to initialize using measurement
-					if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
-								      sample.epv)) {
+					if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
+								      sq(sample.epv))) {
 						ekf.enableControlStatusAuxGpos();
 						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
 						_state = State::active;

diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
@@ -191,9 +191,9 @@ class Ekf final : public EstimatorInterface
 	void getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
 	bool checkLatLonValidity(double latitude, double longitude);
 	bool checkAltitudeValidity(float altitude);
-	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN);
-	bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float eph = NAN,
-				   float epv = NAN);
+	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float hpos_var = NAN, float vpos_var = NAN);
+	bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float hpos_var = NAN,
+				   float vpos_var = NAN);
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
 	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;

diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp
@@ -383,7 +383,7 @@ TEST_F(EkfFlowTest, deadReckoning)
 	const float altitude_new  = 1500.0;
 	const float eph = 50.f;
 	const float epv = 10.f;
-	_ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new, eph, epv);
+	_ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new, eph * eph, epv * epv);
 
 	const Vector3f lpos_after_reset = _ekf->getPosition();