ekf2: fix double precision observation in aid_src topic

PX4 · Oct 28, 2024 · b300f60 · b300f60
1 parent 5fe322f
commit b300f60
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 21 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
@@ -74,26 +74,21 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 		}
 
 		estimator_aid_source2d_s aid_src{};
-		const LatLonAlt position(sample.latitude, sample.longitude, 0.f); //TODO: use ekf altitude?
-		const Vector2f innovation = (ekf.getLatLonAlt() - position).xy();
+		const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
+		const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used
 
 		// relax the upper observation noise limit which prevents bad measurements perturbing the position estimate
 		float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f);
 		const float pos_var = sq(pos_noise);
 		const Vector2f pos_obs_var(pos_var, pos_var);
 
 		ekf.updateAidSourceStatus(aid_src,
-					  sample.time_us,                                    // sample timestamp
-					  Vector2f(sample.latitude, sample.longitude),       // observation //TODO: warning float
-					  pos_obs_var,                                       // observation variance
-					  innovation,                                        // innovation
-					  Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
-					  math::max(_param_ekf2_agp_gate.get(), 1.f));       // innovation gate
-
-		// Override float lat/lon to get double precision
-		// TODO: fix updateAidSourceStatus to accept double precision observations
-		aid_src.observation[0] = sample.latitude;
-		aid_src.observation[1] = sample.longitude;
+					  sample.time_us,                                      // sample timestamp
+					  matrix::Vector2d(sample.latitude, sample.longitude), // observation
+					  pos_obs_var,                                         // observation variance
+					  innovation,                                          // innovation
+					  Vector2f(ekf.getPositionVariance()) + pos_obs_var,   // innovation variance
+					  math::max(_param_ekf2_agp_gate.get(), 1.f));         // innovation gate
 
 		const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
 						 && ekf.control_status_flags().yaw_align;

diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp
@@ -241,16 +241,11 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
 
 	updateAidSourceStatus(aid_src,
 			      gnss_sample.time_us,                                    // sample timestamp
-			      Vector2f(gnss_sample.lat, gnss_sample.lon),             // observation <== warning float
+			      matrix::Vector2d(gnss_sample.lat, gnss_sample.lon),     // observation
 			      pos_obs_var,                                            // observation variance
 			      innovation,                                             // innovation
 			      Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
 			      math::max(_params.gps_pos_innov_gate, 1.f));            // innovation gate
-
-	// Override float lat/lon to get double precision
-	// TODO: fix updateAidSourceStatus to accept double precision observations
-	aid_src.observation[0] = gnss_sample.lat;
-	aid_src.observation[1] = gnss_sample.lon;
 }
 
 void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)

diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
@@ -1061,9 +1061,9 @@ class Ekf final : public EstimatorInterface
 	}
 
 	// helper used for populating and filtering estimator aid source struct for logging
-	template <typename T, typename S>
+	template <typename T, typename S, typename D>
 	void updateAidSourceStatus(T &status, const uint64_t &timestamp_sample,
-				   const S &observation, const S &observation_variance,
+				   const D &observation, const S &observation_variance,
 				   const S &innovation, const S &innovation_variance,
 				   float innovation_gate = 1.f) const
 	{