EKF2: ellipsoidal earth navigation

msg/EstimatorAidSource2d.msg

@@ -7,7 +7,7 @@ uint32 device_id
 
 uint64 time_last_fuse
 
-float32[2] observation
+float64[2] observation
 float32[2] observation_variance
 
 float32[2] innovation

src/modules/ekf2/CMakeLists.txt

@@ -277,6 +277,7 @@ px4_add_module(
 		world_magnetic_model
 
 		${EKF_LIBS}
+		lat_lon_alt
 		bias_estimator
 		output_predictor
 	UNITY_BUILD

src/modules/ekf2/EKF/CMakeLists.txt

@@ -33,6 +33,7 @@
 
 add_subdirectory(bias_estimator)
 add_subdirectory(output_predictor)
+add_subdirectory(lat_lon_alt)
 
 set(EKF_LIBS)
 set(EKF_SRCS)
@@ -154,6 +155,7 @@ target_link_libraries(ecl_EKF
 	PRIVATE
 		bias_estimator
 		geo
+		lat_lon_alt
 		output_predictor
 		world_magnetic_model
 		${EKF_LIBS}

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp

@@ -74,24 +74,21 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 		}
 
 		estimator_aid_source2d_s aid_src{};
-		Vector2f position;
-
-		if (ekf.global_origin_valid()) {
-			position = ekf.global_origin().project(sample.latitude, sample.longitude);
-			//const float hgt = ekf.getEkfGlobalOriginAltitude() - (float)sample.altitude;
-			// 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
-						  position,                                          // observation
-						  pos_obs_var,                                       // observation variance
-						  Vector2f(ekf.state().pos) - position,              // innovation
-						  Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
-						  math::max(_param_ekf2_agp_gate.get(), 1.f));       // innovation gate
-		}
+		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
+					  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;
@@ -113,8 +110,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 
 				} else {
 					// Try to initialize using measurement
-					if (ekf.setEkfGlobalOriginFromCurrentPos(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
-							sample.epv)) {
+					if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
+								      sample.epv)) {
 						ekf.enableControlStatusAuxGpos();
 						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
 						_state = State::active;
@@ -131,7 +128,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 				if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max)
 				    || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
 
-					ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
+					ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
 
 					ekf.resetAidSourceStatusZeroInnovation(aid_src);
 

src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp

@@ -73,7 +73,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 
 			if (_baro_counter <= _obs_buffer_length) {
 				// Initialize the pressure offset (included in the baro bias)
-				bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
+				bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
 			}
 		}
 
@@ -106,7 +106,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 		if (measurement_valid) {
 			bias_est.setMaxStateNoise(sqrtf(measurement_var));
 			bias_est.setProcessNoiseSpectralDensity(_params.baro_bias_nsd);
-			bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
+			bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
 		}
 
 		// determine if we should use height aiding
@@ -131,8 +131,8 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 					ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
 
 					_information_events.flags.reset_hgt_to_baro = true;
-					resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var);
-					bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
+					resetHeightTo(_baro_lpf.getState() - bias_est.getBias(), measurement_var);
+					bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
 
 					// reset vertical velocity if no valid sources available
 					if (!isVerticalVelocityAidingActive()) {
@@ -163,12 +163,12 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 					_height_sensor_ref = HeightSensor::BARO;
 
 					_information_events.flags.reset_hgt_to_baro = true;
-					resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var);
-					bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
+					initialiseAltitudeTo(measurement, measurement_var);
+					bias_est.reset();
 
 				} else {
 					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
-					bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
+					bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
 				}
 
 				aid_src.time_last_fuse = imu_sample.time_us;

src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp

@@ -99,7 +99,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 	if (measurement_valid && quality_sufficient) {
 		bias_est.setMaxStateNoise(sqrtf(measurement_var));
 		bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd);
-		bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
+		bias_est.fuseBias(measurement + _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
 	}
 
 	const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
@@ -117,11 +117,11 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 
 					if (_height_sensor_ref == HeightSensor::EV) {
 						_information_events.flags.reset_hgt_to_ev = true;
-						resetVerticalPositionTo(measurement, measurement_var);
+						resetHeightTo(-measurement, measurement_var);
 						bias_est.reset();
 
 					} else {
-						bias_est.setBias(-_state.pos(2) + measurement);
+						bias_est.setBias(_gpos.altitude() + measurement);
 					}
 
 					aid_src.time_last_fuse = _time_delayed_us;
@@ -146,8 +146,8 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 				// All height sources are failing
 				ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME);
 				_information_events.flags.reset_hgt_to_ev = true;
-				resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var);
-				bias_est.setBias(-_state.pos(2) + measurement);
+				resetHeightTo(-measurement - bias_est.getBias(), measurement_var);
+				bias_est.setBias(_gpos.altitude() + measurement);
 
 				aid_src.time_last_fuse = _time_delayed_us;
 
@@ -170,14 +170,14 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 			if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::EV)) {
 				ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
 				_information_events.flags.reset_hgt_to_ev = true;
-				resetVerticalPositionTo(measurement, measurement_var);
+				resetHeightTo(-measurement, measurement_var);
 
 				_height_sensor_ref = HeightSensor::EV;
 				bias_est.reset();
 
 			} else {
 				ECL_INFO("starting %s fusion", AID_SRC_NAME);
-				bias_est.setBias(-_state.pos(2) + measurement);
+				bias_est.setBias(_gpos.altitude() + measurement);
 			}
 
 			aid_src.time_last_fuse = _time_delayed_us;

src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp

@@ -137,6 +137,8 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 
 #endif // CONFIG_EKF2_GNSS
 
+	const Vector2f position_estimate = getLocalHorizontalPosition();
+
 	const Vector2f measurement{pos(0), pos(1)};
 
 	const Vector2f measurement_var{
@@ -150,7 +152,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 	if (!bias_fusion_was_active && _ev_pos_b_est.fusionActive()) {
 		if (quality_sufficient) {
 			// reset the bias estimator
-			_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+			_ev_pos_b_est.setBias(-position_estimate + measurement);
 
 		} else if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.ev_pos)) {
 			// otherwise stop EV position, when quality is good again it will restart with reset bias
@@ -165,7 +167,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 			      ev_sample.time_us,                                      // sample timestamp
 			      position,                                               // observation
 			      pos_obs_var,                                            // observation variance
-			      Vector2f(_state.pos) - position,                        // innovation
+			      position_estimate - position,                           // innovation
 			      Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
 			      math::max(_params.ev_pos_innov_gate, 1.f));             // innovation gate
 
@@ -174,7 +176,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 	if (measurement_valid && quality_sufficient) {
 		_ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1))));
 		_ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO
-		_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()),
+		_ev_pos_b_est.fuseBias(measurement - position_estimate,
 				       measurement_var + Vector2f(getStateVariance<State::pos>()));
 	}
 
@@ -213,7 +215,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem
 	// TODO:  (_params.position_sensor_ref == PositionSensor::EV)
 	if (_control_status.flags.gps) {
 		ECL_INFO("starting %s fusion", EV_AID_SRC_NAME);
-		_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+		_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
 		_ev_pos_b_est.setFusionActive();
 
 	} else {
@@ -245,7 +247,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
 				_ev_pos_b_est.reset();
 
 			} else {
-				_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+				_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
 			}
 
 			aid_src.time_last_fuse = _time_delayed_us;
@@ -275,14 +277,14 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
 
 			if (_control_status.flags.gps && !pos_xy_fusion_failing) {
 				// reset EV position bias
-				_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+				_ev_pos_b_est.setBias(-Vector2f(getLocalHorizontalPosition()) + measurement);
 
 			} else {
 				_information_events.flags.reset_pos_to_vision = true;
 
 				if (_control_status.flags.gps) {
 					resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
-					_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
+					_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
 
 				} else {
 					resetHorizontalPositionTo(measurement, measurement_var);

src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp

@@ -51,7 +51,7 @@ void Ekf::controlFakeHgtFusion()
 		const float obs_var = sq(_params.pos_noaid_noise);
 		const float innov_gate = 3.f;
 
-		updateVerticalPositionAidStatus(aid_src, _time_delayed_us, _last_known_pos(2), obs_var, innov_gate);
+		updateVerticalPositionAidStatus(aid_src, _time_delayed_us, -_last_known_gpos.altitude(), obs_var, innov_gate);
 
 		const bool continuing_conditions_passing = !isVerticalAidingActive();
 		const bool starting_conditions_passing = continuing_conditions_passing
@@ -98,7 +98,7 @@ void Ekf::controlFakeHgtFusion()
 void Ekf::resetFakeHgtFusion()
 {
 	ECL_INFO("reset fake height fusion");
-	_last_known_pos(2) = _state.pos(2);
+	_last_known_gpos.setAltitude(_gpos.altitude());
 
 	resetVerticalVelocityToZero();
 	resetHeightToLastKnown();
@@ -109,8 +109,8 @@ void Ekf::resetFakeHgtFusion()
 void Ekf::resetHeightToLastKnown()
 {
 	_information_events.flags.reset_pos_to_last_known = true;
-	ECL_INFO("reset height to last known (%.3f)", (double)_last_known_pos(2));
-	resetVerticalPositionTo(_last_known_pos(2), sq(_params.pos_noaid_noise));
+	ECL_INFO("reset height to last known (%.3f)", (double)_last_known_gpos.altitude());
+	resetHeightTo(_last_known_gpos.altitude(), sq(_params.pos_noaid_noise));
 }
 
 void Ekf::stopFakeHgtFusion()

src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp

@@ -63,17 +63,17 @@ void Ekf::controlFakePosFusion()
 			obs_var(0) = obs_var(1) = sq(0.5f);
 		}
 
-		const Vector2f position(_last_known_pos);
+		const Vector2f innovation = (_gpos - _last_known_gpos).xy();
 
 		const float innov_gate = 3.f;
 
 		updateAidSourceStatus(aid_src,
 				      _time_delayed_us,
-				      position,                                           // observation
-				      obs_var,                                            // observation variance
-				      Vector2f(_state.pos) - position,                    // innovation
-				      Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
-				      innov_gate);                                        // innovation gate
+				      Vector2f(_gpos.latitude_deg(), _gpos.longitude_deg()), // observation
+				      obs_var,                                               // observation variance
+				      innovation,                       // innovation
+				      Vector2f(getStateVariance<State::pos>()) + obs_var,    // innovation variance
+				      innov_gate);                                           // innovation gate
 
 		const bool enable_valid_fake_pos = _control_status.flags.constant_pos || _control_status.flags.vehicle_at_rest;
 		const bool enable_fake_pos = !enable_valid_fake_pos
@@ -95,7 +95,7 @@ void Ekf::controlFakePosFusion()
 void Ekf::resetFakePosFusion()
 {
 	ECL_INFO("reset fake position fusion");
-	_last_known_pos.xy() = _state.pos.xy();
+	_last_known_gpos.setLatLon(_gpos);
 
 	resetHorizontalPositionToLastKnown();
 	resetHorizontalVelocityToZero();

src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp

@@ -62,9 +62,9 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 
 		const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
 		const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-		const float gnss_alt = _gps_sample_delayed.alt + pos_offset_earth(2);
+		const float gnss_alt = gps_sample.alt + pos_offset_earth(2);
 
-		const float measurement = gnss_alt - getEkfGlobalOriginAltitude();
+		const float measurement = gnss_alt;
 		const float measurement_var = sq(noise);
 
 		const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
@@ -81,13 +81,13 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 		if (measurement_valid) {
 			bias_est.setMaxStateNoise(sqrtf(measurement_var));
 			bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd);
-			bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
+			bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
 		}
 
 		// determine if we should use height aiding
 		const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
 				&& measurement_valid
-				&& _pos_ref.isInitialized()
+				&& _local_origin_lat_lon.isInitialized()
 				&& _gps_checks_passed;
 
 		const bool starting_conditions_passing = continuing_conditions_passing
@@ -105,8 +105,8 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 					ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
 
 					_information_events.flags.reset_hgt_to_gps = true;
-					resetVerticalPositionTo(aid_src.observation, measurement_var);
-					bias_est.setBias(_state.pos(2) + measurement);
+					resetHeightTo(measurement, measurement_var);
+					bias_est.setBias(-_gpos.altitude() + measurement);
 
 					aid_src.time_last_fuse = _time_delayed_us;
 
@@ -128,13 +128,13 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 					_height_sensor_ref = HeightSensor::GNSS;
 
 					_information_events.flags.reset_hgt_to_gps = true;
-					resetVerticalPositionTo(-measurement, measurement_var);
-					_gpos_origin_epv = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty
+
+					initialiseAltitudeTo(measurement, measurement_var);
 					bias_est.reset();
 
 				} else {
 					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
-					bias_est.setBias(_state.pos(2) + measurement);
+					bias_est.setBias(-_gpos.altitude() + measurement);
 				}
 
 				aid_src.time_last_fuse = _time_delayed_us;