include timestamp_sample into target gnss message

Author: jonas

msg/TargetGnss.msg

@@ -1,6 +1,7 @@
 # landing target GNSS position in WGS84 coordinates.
 
 uint64 timestamp	# [us] time since system start
+uint64 timestamp_sample       # [us] timestamp of the raw observation
 
 float64 latitude_deg	# [deg] Latitude, allows centimeter level RTK precision
 float64 longitude_deg	# [deg] Longitude, allows centimeter level RTK precision

src/modules/mavlink/mavlink_receiver.cpp

@@ -2857,7 +2857,9 @@ MavlinkReceiver::handle_message_target_absolute(mavlink_message_t *msg)
 	mavlink_msg_target_absolute_decode(msg, &target_absolute);
 
 	target_gnss_s target_GNSS_report{};
-	target_GNSS_report.timestamp = _mavlink_timesync.sync_stamp(target_absolute.timestamp);
+
+	target_GNSS_report.timestamp = hrt_absolute_time();
+	target_GNSS_report.timestamp_sample = _mavlink_timesync.sync_stamp(target_absolute.timestamp);
 
 	bool updated = false;
 

src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp

@@ -694,6 +694,7 @@ SimulatorMavlink::handle_message_target_absolute(const mavlink_message_t *msg)
 
 	target_gnss_s target_GNSS_report{};
 	target_GNSS_report.timestamp = hrt_absolute_time();
+	target_GNSS_report.timestamp_sample = target_GNSS_report.timestamp;
 
 	bool updated = false;
 

src/modules/vision_target_estimator/Orientation/KF_orientation.cpp

@@ -48,6 +48,10 @@ namespace vision_target_estimator
 
 void KF_orientation::predict(float dt)
 {
+	if (!PX4_ISFINITE(dt) || (dt < 0.f)) {
+		return;
+	}
+
 	matrix::Vector<float, State::size> state_next;
 	matrix::SquareMatrix<float, State::size> cov_next;
 
@@ -104,8 +108,16 @@ void KF_orientation::computeInnovation(const matrix::Vector<float, State::size>
 				       const ScalarMeas &meas,
 				       float &innov, float &innov_var) const
 {
-	innov = matrix::wrap_pi(meas.val - (meas.H.transpose() * state)(0, 0));
+
+	innov = meas.val - (meas.H.transpose() * state)(0, 0);
 	innov_var = (meas.H.transpose() * cov * meas.H)(0, 0) + meas.unc;
+
+	// Wrap innovations for yaw measurements
+	const bool is_yaw_meas = (fabsf(meas.H(State::yaw)) > 0.f) && (fabsf(meas.H(State::yaw_rate)) < FLT_EPSILON);
+
+	if (is_yaw_meas) {
+		innov = matrix::wrap_pi(innov);
+	}
 }
 
 void KF_orientation::pushHistory(const uint64_t time_us)

src/modules/vision_target_estimator/Orientation/KF_orientation.h

@@ -96,6 +96,7 @@ class KF_orientation
 	};
 
 	const matrix::Vector<float, State::size> &getState() const { return _state; }
+	const matrix::SquareMatrix<float, State::size> &getStateCovariance() const { return _state_covariance; }
 	matrix::Vector<float, State::size> getStateCovarianceDiag() const { return _state_covariance.diag(); }
 
 	void setYawAccVar(float var) { _yaw_acc_var = var; }

src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp

@@ -299,12 +299,21 @@ void VTEOrientation::updateParams()
 
 	ModuleParams::updateParams();
 
-	_yaw_unc = _param_vte_yaw_unc_in.get();
+	const float new_yaw_unc = _param_vte_yaw_unc_in.get();
 	const float new_yaw_acc_var = _param_vte_yaw_acc_unc.get();
 	const float new_ev_angle_noise = _param_vte_ev_angle_noise.get();
 	_ev_noise_md = _param_vte_ev_noise_md.get();
 	const float new_nis_threshold = _param_vte_yaw_nis_thre.get();
 
+	if (PX4_ISFINITE(new_yaw_unc) && new_yaw_unc > 0.f) {
+		const bool yaw_unc_changed = PX4_ISFINITE(_yaw_unc) && (fabsf(new_yaw_unc - _yaw_unc) > 1e-3f);
+		_yaw_unc = new_yaw_unc;
+
+		if (_estimator_initialized && yaw_unc_changed) {
+			resetFilter();
+		}
+	}
+
 	if (PX4_ISFINITE(new_yaw_acc_var) && (new_yaw_acc_var >= 0.f)) {
 		_yaw_acc_var = new_yaw_acc_var;
 		_target_est_yaw.setYawAccVar(_yaw_acc_var);

src/modules/vision_target_estimator/Position/KF_position.cpp

@@ -52,6 +52,11 @@ namespace vision_target_estimator
 
 void KF_position::predict(float dt, float acc_uav)
 {
+
+	if (!PX4_ISFINITE(dt) || (dt < 0.f) || !PX4_ISFINITE(acc_uav)) {
+		return;
+	}
+
 	_last_acc = acc_uav;
 
 	matrix::Vector<float, vtest::State::size> state_next;

src/modules/vision_target_estimator/Position/KF_position.h

@@ -91,6 +91,7 @@ class KF_position
 	};
 
 	const matrix::Vector<float, vtest::State::size> &getState() const { return _state; }
+	const matrix::SquareMatrix<float, vtest::State::size> &getStateCovariance() const { return _state_covariance; }
 	matrix::Vector<float, vtest::State::size> getStateCovarianceDiag() const { return _state_covariance.diag(); }
 
 	void setInputVar(float var) { _input_var = var; }

src/modules/vision_target_estimator/Position/VTEPosition.cpp

@@ -449,13 +449,13 @@ bool VTEPosition::updateUavGpsData()
 		_uav_gps_position.lat_deg = vehicle_gps_position.latitude_deg;
 		_uav_gps_position.lon_deg = vehicle_gps_position.longitude_deg;
 		_uav_gps_position.alt_m = (float)vehicle_gps_position.altitude_msl_m;
-		_uav_gps_position.timestamp = vehicle_gps_position.timestamp;
+		_uav_gps_position.timestamp = vehicle_gps_position.timestamp_sample;
 		_uav_gps_position.eph = vehicle_gps_position.eph;
 		_uav_gps_position.epv = vehicle_gps_position.epv;
 		_uav_gps_position.valid = isUavGpsPositionValid();
 
 		// Velocity
-		_uav_gps_vel.timestamp = vehicle_gps_position.timestamp;
+		_uav_gps_vel.timestamp = vehicle_gps_position.timestamp_sample;
 		_uav_gps_vel.xyz(vtest::Axis::x) = vehicle_gps_position.vel_n_m_s;
 		_uav_gps_vel.xyz(vtest::Axis::y) = vehicle_gps_position.vel_e_m_s;
 		_uav_gps_vel.xyz(vtest::Axis::z) = vehicle_gps_position.vel_d_m_s;
@@ -474,8 +474,8 @@ bool VTEPosition::updateUavGpsData()
 #if defined(CONFIG_VTEST_MOVING)
 void VTEPosition::updateTargetGpsVelocity(const target_gnss_s &target_gnss)
 {
-	_target_gps_vel.timestamp = target_gnss.timestamp;
-	_target_gps_vel.valid = isMeasRecent(target_gnss.timestamp);
+	_target_gps_vel.timestamp = target_gnss.timestamp_sample;
+	_target_gps_vel.valid = isMeasRecent(target_gnss.timestamp_sample);
 
 	_target_gps_vel.xyz(vtest::Axis::x) = target_gnss.vel_n_m_s;
 	_target_gps_vel.xyz(vtest::Axis::y) = target_gnss.vel_e_m_s;
@@ -485,7 +485,7 @@ void VTEPosition::updateTargetGpsVelocity(const target_gnss_s &target_gnss)
 
 bool VTEPosition::isTargetGpsPositionValid(const target_gnss_s &target_gnss)
 {
-	if (!isMeasRecent(target_gnss.timestamp)) {
+	if (!isMeasRecent(target_gnss.timestamp_sample)) {
 		return false;
 	}
 
@@ -499,7 +499,7 @@ bool VTEPosition::isTargetGpsPositionValid(const target_gnss_s &target_gnss)
 
 bool VTEPosition::isTargetGpsVelocityValid(const target_gnss_s &target_gnss)
 {
-	if (!isMeasRecent(target_gnss.timestamp)) {
+	if (!isMeasRecent(target_gnss.timestamp_sample)) {
 		return false;
 	}
 
@@ -874,7 +874,7 @@ bool VTEPosition::ProcessObsGNSSVelTarget(const target_gnss_s &target_gnss, Targ
 	obs.meas_h_xyz(vtest::Axis::y, vtest::State::vel_target) = 1;
 	obs.meas_h_xyz(vtest::Axis::z, vtest::State::vel_target) = 1;
 
-	obs.timestamp = target_gnss.timestamp;
+	obs.timestamp = target_gnss.timestamp_sample;
 
 	obs.type = ObsType::Target_gps_vel;
 
@@ -953,7 +953,7 @@ bool VTEPosition::processObsGNSSPosMission(TargetObs &obs)
 /*Target GNSS observation: [rx + bx, ry + by, rz + bz]*/
 bool VTEPosition::processObsGNSSPosTarget(const target_gnss_s &target_gnss, TargetObs &obs)
 {
-	const int64_t time_diff_us = signedTimeDiffUs(target_gnss.timestamp, _uav_gps_position.timestamp);
+	const int64_t time_diff_us = signedTimeDiffUs(target_gnss.timestamp_sample, _uav_gps_position.timestamp);
 	const float dt_sync_us = fabsf(static_cast<float>(time_diff_us));
 
 	if (dt_sync_us > _meas_recent_timeout_us) {
@@ -1030,7 +1030,7 @@ bool VTEPosition::processObsGNSSPosTarget(const target_gnss_s &target_gnss, Targ
 		obs.meas_h_xyz(vtest::Axis::z, vtest::State::bias) = 1;
 	}
 
-	obs.timestamp = target_gnss.timestamp;
+	obs.timestamp = target_gnss.timestamp_sample;
 
 	obs.meas_xyz = gps_relative_pos;