EKF2 limit map reprojection (#7900)

msg/vehicle_global_position.msg

@@ -8,7 +8,6 @@ uint64 time_utc_usec		# GPS UTC timestamp, (microseconds)
 float64 lat			# Latitude, (degrees)
 float64 lon			# Longitude, (degrees)
 float32 alt			# Altitude AMSL, (meters)
-float64[2] delta_lat_lon # Reset delta for horizontal position coordinates
 float32 delta_alt 	# Reset delta for altitude
 uint8 lat_lon_reset_counter	# Counter for reset events on horizontal position coordinates
 uint8 alt_reset_counter 		# Counter for reset events on altitude

src/modules/ekf2/ekf2_main.cpp

@@ -41,29 +41,18 @@
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <px4_module.h>
-#include <px4_tasks.h>
 #include <px4_posix.h>
+#include <px4_tasks.h>
 #include <px4_time.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <time.h>
-#include <float.h>
-
-#include <arch/board/board.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
+
+#include <cfloat>
+
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
-#include <platforms/px4_defines.h>
 #include <drivers/drv_hrt.h>
 #include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
 
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -95,8 +84,7 @@ class Ekf2 : public control::SuperBlock, public ModuleBase<Ekf2>
 {
 public:
 	Ekf2();
-
-	~Ekf2();
+	~Ekf2() override = default;
 
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -120,16 +108,12 @@ public:
 	int print_status() override;
 
 private:
-	static constexpr float _dt_max = 0.02;	///< minimum allowed arrival time between non-IMU sensor readings  (sec)
-	bool 	_replay_mode;			///< true when we use replay data from a log
-	int32_t _publish_replay_mode;		///< set to 1 if we should publish replay messages for logging
-
-	float	_default_ev_pos_noise = 0.05f;	///< external vision position noise used when an invalid value is supplied (m)
-	float	_default_ev_ang_noise = 0.05f;	///< external vision angle noise used when an invalid value is supplied (rad)
+	bool 	_replay_mode = false;			///< true when we use replay data from a log
 
 	// time slip monitoring
-	uint64_t integrated_time_us = 0;	///< integral of gyro delta time from start (uSec)
-	uint64_t start_time_us = 0;		///< system time at EKF start (uSec)
+	uint64_t _integrated_time_us = 0;	///< integral of gyro delta time from start (uSec)
+	uint64_t _start_time_us = 0;		///< system time at EKF start (uSec)
+	uint64_t _last_time_slip_us = 0;	///< Last time slip (uSec)
 
 	// Initialise time stamps used to send sensor data to the EKF and for logging
 	uint64_t _timestamp_mag_us = 0;		///< magnetomer data timestamp (uSec)
@@ -155,31 +139,35 @@ private:
 	// Used to check, save and use learned magnetometer biases
 	hrt_abstime _last_magcal_us = 0;	///< last time the EKF was operating a mode that estimates magnetomer biases (uSec)
 	hrt_abstime _total_cal_time_us = 0;	///< accumulated calibration time since the last save
-	hrt_abstime _last_time_slip_us = 0;	///< Last time slip (uSec)
+
 	float _last_valid_mag_cal[3] = {};	///< last valid XYZ magnetometer bias estimates (mGauss)
 	bool _valid_cal_available[3] = {};	///< true when an unsaved valid calibration for the XYZ magnetometer bias is available
 	float _last_valid_variance[3] = {};	///< variances for the last valid magnetometer XYZ bias estimates (mGauss**2)
 
 	// Used to filter velocity innovations during pre-flight checks
+	bool _vel_innov_preflt_fail = false;	///< true if the norm of the filtered innovation vector is too large before flight
 	Vector3f _vel_innov_lpf_ned = {};	///< Preflight low pass filtered velocity innovations (m/sec)
 	float _hgt_innov_lpf = 0.0f;		///< Preflight low pass filtered height innovation (m)
-	const float _innov_lpf_tau_inv = 0.2f;	///< Preflight low pass filter time constant inverse (1/sec)
-	const float _vel_innov_test_lim = 0.5f;	///< Maximum permissible velocity innovation to pass pre-flight checks (m/sec)
-	const float _hgt_innov_test_lim = 1.5f;	///< Maximum permissible height innovation to pass pre-flight checks (m)
+
+	static constexpr float _innov_lpf_tau_inv = 0.2f;	///< Preflight low pass filter time constant inverse (1/sec)
+	static constexpr float _vel_innov_test_lim =
+		0.5f;	///< Maximum permissible velocity innovation to pass pre-flight checks (m/sec)
+	static constexpr float _hgt_innov_test_lim =
+		1.5f;	///< Maximum permissible height innovation to pass pre-flight checks (m)
 	const float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim;	///< preflight velocity innovation spike limit (m/sec)
 	const float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim;	///< preflight position innovation spike limit (m)
-	bool _vel_innov_preflt_fail = false;	///< true if the norm of the filtered innovation vector is too large before flight
 
 	orb_advert_t _att_pub;
-	orb_advert_t _lpos_pub;
 	orb_advert_t _control_state_pub;
-	orb_advert_t _vehicle_global_position_pub;
 	orb_advert_t _wind_pub;
 	orb_advert_t _estimator_status_pub;
 	orb_advert_t _estimator_innovations_pub;
 	orb_advert_t _replay_pub;
 	orb_advert_t _ekf2_timestamps_pub;
 
+	uORB::Publication<vehicle_local_position_s> _vehicle_local_position_pub;
+	uORB::Publication<vehicle_global_position_s> _vehicle_global_position_pub;
+
 	/* Low pass filter for attitude rates */
 	math::LowPassFilter2p _lp_roll_rate;	///< Low pass filter applied to roll rates published on the control_state message
 	math::LowPassFilter2p _lp_pitch_rate;	///< Low pass filter applied to pitch rates published on the control_state message
@@ -246,7 +234,8 @@ private:
 	control::BlockParamExtFloat _requiredGDoP;	///< maximum acceptable geometric dilution of precision
 	control::BlockParamExtFloat _requiredHdrift;	///< maximum acceptable horizontal drift speed (m/s)
 	control::BlockParamExtFloat _requiredVdrift;	///< maximum acceptable vertical drift speed (m/s)
-	control::BlockParamExtInt _param_record_replay_msg;	///< turns on recording of ekf2 replay messages
+
+	control::BlockParamInt _param_record_replay_msg;	///< turns on recording of ekf2 replay messages
 
 	// measurement source control
 	control::BlockParamExtInt
@@ -267,8 +256,8 @@ private:
 	_rng_aid_innov_gate;	///< gate size used for innovation consistency checks for range aid fusion (STD)
 
 	// vision estimate fusion
-	control::BlockParamExtFloat _ev_pos_noise;	///< default position observation noise for exernal vision measurements (m)
-	control::BlockParamExtFloat _ev_ang_noise;	///< default angular observation noise for exernal vision measurements (rad)
+	control::BlockParamFloat _ev_pos_noise;	///< default position observation noise for exernal vision measurements (m)
+	control::BlockParamFloat _ev_ang_noise;	///< default angular observation noise for exernal vision measurements (rad)
 	control::BlockParamExtFloat _ev_innov_gate;	///< external vision position innovation consistency gate size (STD)
 
 	// optical flow fusion
@@ -345,16 +334,15 @@ private:
 Ekf2::Ekf2():
 	SuperBlock(nullptr, "EKF"),
 	_replay_mode(false),
-	_publish_replay_mode(0),
 	_att_pub(nullptr),
-	_lpos_pub(nullptr),
 	_control_state_pub(nullptr),
-	_vehicle_global_position_pub(nullptr),
 	_wind_pub(nullptr),
 	_estimator_status_pub(nullptr),
 	_estimator_innovations_pub(nullptr),
 	_replay_pub(nullptr),
 	_ekf2_timestamps_pub(nullptr),
+	_vehicle_local_position_pub(ORB_ID(vehicle_local_position), -1, &getPublications()),
+	_vehicle_global_position_pub(ORB_ID(vehicle_global_position), -1, &getPublications()),
 	_lp_roll_rate(250.0f, 30.0f),
 	_lp_pitch_rate(250.0f, 30.0f),
 	_lp_yaw_rate(250.0f, 20.0f),
@@ -403,7 +391,7 @@ Ekf2::Ekf2():
 	_requiredGDoP(this, "EKF2_REQ_GDOP", false, _params->req_gdop),
 	_requiredHdrift(this, "EKF2_REQ_HDRIFT", false, _params->req_hdrift),
 	_requiredVdrift(this, "EKF2_REQ_VDRIFT", false, _params->req_vdrift),
-	_param_record_replay_msg(this, "EKF2_REC_RPL", false, _publish_replay_mode),
+	_param_record_replay_msg(this, "EKF2_REC_RPL", false),
 	_fusion_mode(this, "EKF2_AID_MASK", false, _params->fusion_mode),
 	_vdist_sensor_type(this, "EKF2_HGT_MODE", false, _params->vdist_sensor_type),
 	_range_noise(this, "EKF2_RNG_NOISE", false, _params->range_noise),
@@ -415,8 +403,8 @@ Ekf2::Ekf2():
 	_rng_aid_hor_vel_max(this, "EKF2_RNG_A_VMAX", false, _params->max_vel_for_range_aid),
 	_rng_aid_height_max(this, "EKF2_RNG_A_HMAX", false, _params->max_hagl_for_range_aid),
 	_rng_aid_innov_gate(this, "EKF2_RNG_A_IGATE", false, _params->range_aid_innov_gate),
-	_ev_pos_noise(this, "EKF2_EVP_NOISE", false, _default_ev_pos_noise),
-	_ev_ang_noise(this, "EKF2_EVA_NOISE", false, _default_ev_ang_noise),
+	_ev_pos_noise(this, "EKF2_EVP_NOISE", false),
+	_ev_ang_noise(this, "EKF2_EVA_NOISE", false),
 	_ev_innov_gate(this, "EKF2_EV_GATE", false, _params->ev_innov_gate),
 	_flow_noise(this, "EKF2_OF_N_MIN", false, _params->flow_noise),
 	_flow_noise_qual_min(this, "EKF2_OF_N_MAX", false, _params->flow_noise_qual_min),
@@ -461,12 +449,6 @@ Ekf2::Ekf2():
 	_K_pstatic_coef_y(this, "EKF2_PCOEF_Y", false),
 	_K_pstatic_coef_z(this, "EKF2_PCOEF_Z", false)
 {
-
-}
-
-Ekf2::~Ekf2()
-{
-
 }
 
 int Ekf2::print_status()
@@ -546,6 +528,8 @@ void Ekf2::run()
 
 		bool gps_updated = false;
 		bool airspeed_updated = false;
+		bool baro_updated = false;
+		bool sensor_selection_updated = false;
 		bool optical_flow_updated = false;
 		bool range_finder_updated = false;
 		bool vehicle_land_detected_updated = false;
@@ -574,6 +558,18 @@ void Ekf2::run()
 			orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
 		}
 
+		orb_check(sensor_baro_sub, &baro_updated);
+
+		if (baro_updated) {
+			orb_copy(ORB_ID(sensor_baro), sensor_baro_sub, &sensor_baro);
+		}
+
+		orb_check(sensor_selection_sub, &sensor_selection_updated);
+
+		if (sensor_selection_updated) {
+			orb_copy(ORB_ID(sensor_selection), sensor_selection_sub, &sensor_selection);
+		}
+
 		orb_check(optical_flow_sub, &optical_flow_updated);
 
 		if (optical_flow_updated) {
@@ -619,13 +615,14 @@ void Ekf2::run()
 		gyro_integral[0] = sensors.gyro_rad[0] * gyro_dt;
 		gyro_integral[1] = sensors.gyro_rad[1] * gyro_dt;
 		gyro_integral[2] = sensors.gyro_rad[2] * gyro_dt;
+
 		float accel_integral[3];
 		float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
 		accel_integral[0] = sensors.accelerometer_m_s2[0] * accel_dt;
 		accel_integral[1] = sensors.accelerometer_m_s2[1] * accel_dt;
 		accel_integral[2] = sensors.accelerometer_m_s2[2] * accel_dt;
-		_ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt,
-				gyro_integral, accel_integral);
+
+		_ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt, gyro_integral, accel_integral);
 
 		// read mag data
 		if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
@@ -640,8 +637,6 @@ void Ekf2::run()
 				// Do not reset parmameters when armed to prevent potential time slips casued by parameter set
 				// and notification events
 				// Check if there has been a persistant change in magnetometer ID
-				orb_copy(ORB_ID(sensor_selection), sensor_selection_sub, &sensor_selection);
-
 				if (sensor_selection.mag_device_id != 0 && sensor_selection.mag_device_id != _mag_bias_id.get()) {
 					if (_invalid_mag_id_count < 200) {
 						_invalid_mag_id_count++;
@@ -718,7 +713,6 @@ void Ekf2::run()
 					float balt_data_avg = _balt_data_sum / (float)_balt_sample_count;
 
 					// estimate air density assuming typical 20degC ambient temperature
-					orb_copy(ORB_ID(sensor_baro), sensor_baro_sub, &sensor_baro);
 					const float pressure_to_density = 100.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
 					float rho = pressure_to_density * sensor_baro.pressure;
 					_ekf.set_air_density(rho);
@@ -741,7 +735,7 @@ void Ekf2::run()
 									  _K_pstatic_coef_z.get() * fminf(_vel_body_wind(2) * _vel_body_wind(2), max_airspeed_sq));
 
 					// correct baro measurement using pressure error estimate and assuming sea level gravity
-					balt_data_avg += pstatic_err / (rho * 9.80665f);
+					balt_data_avg += pstatic_err / (rho * CONSTANTS_ONE_G);
 
 					// push to estimator
 					_ekf.setBaroData(1000 * (uint64_t)balt_time_ms, balt_data_avg);
@@ -756,7 +750,7 @@ void Ekf2::run()
 
 		// read gps data if available
 		if (gps_updated) {
-			struct gps_message gps_msg = {};
+			struct gps_message gps_msg;
 			gps_msg.time_usec = gps.timestamp;
 			gps_msg.lat = gps.lat;
 			gps_msg.lon = gps.lon;
@@ -780,19 +774,22 @@ void Ekf2::run()
 
 		// only set airspeed data if condition for airspeed fusion are met
 		bool fuse_airspeed = airspeed_updated && !vehicle_status.is_rotary_wing
-				     && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s && _arspFusionThreshold.get() >= 0.1f;
+				     && (_arspFusionThreshold.get() > FLT_EPSILON)
+				     && (airspeed.true_airspeed_m_s > _arspFusionThreshold.get());
 
 		if (fuse_airspeed) {
 			float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
 			_ekf.setAirspeedData(airspeed.timestamp, airspeed.true_airspeed_m_s, eas2tas);
 		}
 
-		// only fuse synthetic sideslip measurements if conditions are met
-		bool fuse_beta = !vehicle_status.is_rotary_wing && _fuseBeta.get();
-		_ekf.set_fuse_beta_flag(fuse_beta);
+		if (vehicle_status_updated) {
+			// only fuse synthetic sideslip measurements if conditions are met
+			bool fuse_beta = !vehicle_status.is_rotary_wing && (_fuseBeta.get() == 1);
+			_ekf.set_fuse_beta_flag(fuse_beta);
 
-		// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
-		_ekf.set_is_fixed_wing(!vehicle_status.is_rotary_wing);
+			// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
+			_ekf.set_is_fixed_wing(!vehicle_status.is_rotary_wing);
+		}
 
 		if (optical_flow_updated) {
 			flow_message flow;
@@ -825,8 +822,8 @@ void Ekf2::run()
 			ev_data.quat = q;
 
 			// position measurement error from parameters. TODO : use covariances from topic
-			ev_data.posErr = _default_ev_pos_noise;
-			ev_data.angErr = _default_ev_ang_noise;
+			ev_data.posErr = _ev_pos_noise.get();
+			ev_data.angErr = _ev_ang_noise.get();
 
 			// use timestamp from external computer, clocks are synchronized when using MAVROS
 			_ekf.setExtVisionData(vision_position_updated ? ev_pos.timestamp : ev_att.timestamp, &ev_data);
@@ -843,11 +840,11 @@ void Ekf2::run()
 		if (_ekf.update()) {
 
 			// integrate time to monitor time slippage
-			if (start_time_us == 0) {
-				start_time_us = now;
+			if (_start_time_us == 0) {
+				_start_time_us = now;
 
-			} else if (start_time_us > 0) {
-				integrated_time_us += sensors.gyro_integral_dt;
+			} else if (_start_time_us > 0) {
+				_integrated_time_us += sensors.gyro_integral_dt;
 			}
 
 			matrix::Quaternion<float> q;
@@ -989,13 +986,16 @@ void Ekf2::run()
 			}
 
 			// generate vehicle local position data
-			struct vehicle_local_position_s lpos = {};
+			vehicle_local_position_s &lpos = _vehicle_local_position_pub.get();
+
 			float pos[3] = {};
 
 			lpos.timestamp = now;
 
 			// Position of body origin in local NED frame
 			_ekf.get_position(pos);
+			const float lpos_x_prev = lpos.x;
+			const float lpos_y_prev = lpos.y;
 			lpos.x = (_ekf.local_position_is_valid()) ? pos[0] : 0.0f;
 			lpos.y = (_ekf.local_position_is_valid()) ? pos[1] : 0.0f;
 			lpos.z = pos[2];
@@ -1013,11 +1013,19 @@ void Ekf2::run()
 			lpos.v_z_valid = !_vel_innov_preflt_fail;
 
 			// Position of local NED origin in GPS / WGS84 frame
-			struct map_projection_reference_s ekf_origin = {};
+			map_projection_reference_s ekf_origin = {};
+			uint64_t origin_time = 0;
+
 			// true if position (x,y,z) has a valid WGS-84 global reference (ref_lat, ref_lon, alt)
-			lpos.xy_global = lpos.z_global = _ekf.get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt);
-			lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
-			lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees
+			const bool ekf_origin_valid = _ekf.get_ekf_origin(&origin_time, &ekf_origin, &lpos.ref_alt);
+			lpos.xy_global = ekf_origin_valid;
+			lpos.z_global = ekf_origin_valid;
+
+			if (ekf_origin_valid && (origin_time > lpos.ref_timestamp)) {
+				lpos.ref_timestamp = origin_time;
+				lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
+				lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees
+			}
 
 			// The rotation of the tangent plane vs. geographical north
 			matrix::Eulerf euler(q);
@@ -1047,28 +1055,19 @@ void Ekf2::run()
 			_ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter);
 
 			// publish vehicle local position data
-			if (_lpos_pub == nullptr) {
-				_lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos);
-
-			} else {
-				orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
-			}
+			_vehicle_local_position_pub.update();
 
 			if (_ekf.global_position_is_valid() && !_vel_innov_preflt_fail) {
 				// generate and publish global position data
-				struct vehicle_global_position_s global_pos = {};
+				vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get();
 
 				global_pos.timestamp = now;
 				global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds
 
-				double est_lat, est_lon, lat_pre_reset, lon_pre_reset;
-				map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon);
-				global_pos.lat = est_lat; // Latitude in degrees
-				global_pos.lon = est_lon; // Longitude in degrees
-				map_projection_reproject(&ekf_origin, lpos.x - lpos.delta_xy[0], lpos.y - lpos.delta_xy[1], &lat_pre_reset,
-							 &lon_pre_reset);
-				global_pos.delta_lat_lon[0] = est_lat - lat_pre_reset;
-				global_pos.delta_lat_lon[1] = est_lon - lon_pre_reset;
+				if (fabsf(lpos_x_prev - lpos.x) > FLT_EPSILON || fabsf(lpos_y_prev - lpos.y) > FLT_EPSILON) {
+					map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &global_pos.lat, &global_pos.lon);
+				}
+
 				global_pos.lat_lon_reset_counter = lpos.xy_reset_counter;
 
 				global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters
@@ -1100,17 +1099,12 @@ void Ekf2::run()
 
 				global_pos.pressure_alt = sensors.baro_alt_meter; // Pressure altitude AMSL (m)
 
-				if (_vehicle_global_position_pub == nullptr) {
-					_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
-
-				} else {
-					orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos);
-				}
+				_vehicle_global_position_pub.update();
 			}
 
 			// publish estimator status
 			{
-				struct estimator_status_s status = {};
+				estimator_status_s status;
 				status.timestamp = now;
 				_ekf.get_state_delayed(status.states);
 				_ekf.get_covariances(status.covariances);
@@ -1128,9 +1122,9 @@ void Ekf2::run()
 				_ekf.get_imu_vibe_metrics(status.vibe);
 
 				// monitor time slippage
-				if (start_time_us != 0 && now > start_time_us) {
-					status.time_slip = (float)(1e-6 * ((double)(now - start_time_us) - (double) integrated_time_us));
-					_last_time_slip_us = (now - start_time_us) - integrated_time_us;
+				if (_start_time_us != 0 && now > _start_time_us) {
+					status.time_slip = (float)(1e-6 * ((double)(now - _start_time_us) - (double) _integrated_time_us));
+					_last_time_slip_us = (now - _start_time_us) - _integrated_time_us;
 
 				} else {
 					status.time_slip = 0.0f;
@@ -1218,7 +1212,7 @@ void Ekf2::run()
 				}
 
 				// Publish wind estimate
-				struct wind_estimate_s wind_estimate = {};
+				wind_estimate_s wind_estimate;
 				wind_estimate.timestamp = now;
 				wind_estimate.windspeed_north = status.states[22];
 				wind_estimate.windspeed_east = status.states[23];
@@ -1235,7 +1229,7 @@ void Ekf2::run()
 
 			// publish estimator innovation data
 			{
-				struct ekf2_innovations_s innovations = {};
+				ekf2_innovations_s innovations;
 				innovations.timestamp = now;
 				_ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]);
 				_ekf.get_mag_innov(&innovations.mag_innov[0]);
@@ -1362,11 +1356,8 @@ void Ekf2::run()
 			}
 		}
 
-
 		// publish replay message if in replay mode
-		bool publish_replay_message = (bool)_param_record_replay_msg.get();
-
-		if (publish_replay_message) {
+		if (_param_record_replay_msg.get() == 1) {
 			struct ekf2_replay_s replay = {};
 			replay.timestamp = now;
 			replay.gyro_integral_dt = sensors.gyro_integral_dt;
@@ -1439,9 +1430,9 @@ void Ekf2::run()
 				replay.quat_ev[1] = ev_att.q[1];
 				replay.quat_ev[2] = ev_att.q[2];
 				replay.quat_ev[3] = ev_att.q[3];
-				// TODO : switch to covariances from topic later
-				replay.pos_err = _default_ev_pos_noise;
-				replay.ang_err = _default_ev_ang_noise;
+				// TODO: switch to covariances from topic later
+				replay.pos_err = _ev_pos_noise.get();
+				replay.ang_err = _ev_ang_noise.get();
 
 			} else {
 				replay.ev_timestamp = 0;
@@ -1463,9 +1454,11 @@ void Ekf2::run()
 	orb_unsubscribe(optical_flow_sub);
 	orb_unsubscribe(range_finder_sub);
 	orb_unsubscribe(ev_pos_sub);
+	orb_unsubscribe(ev_att_sub);
 	orb_unsubscribe(vehicle_land_detected_sub);
 	orb_unsubscribe(status_sub);
 	orb_unsubscribe(sensor_selection_sub);
+	orb_unsubscribe(sensor_baro_sub);
 }
 
 Ekf2 *Ekf2::instantiate(int argc, char *argv[])
@@ -1517,7 +1510,7 @@ int Ekf2::task_spawn(int argc, char *argv[])
 	_task_id = px4_task_spawn_cmd("ekf2",
 				      SCHED_DEFAULT,
 				      SCHED_PRIORITY_ESTIMATOR,
-				      5900,
+				      5700,
 				      (px4_main_t)&run_trampoline,
 				      (char *const *)argv);