Interface: output vector quantities by "return-by-value"

EKF/airspeed_fusion.cpp

@@ -225,16 +225,14 @@ void Ekf::fuseAirspeed()
 	}
 }
 
-void Ekf::get_wind_velocity(float *wind)
+Vector2f Ekf::getWindVelocity() const
 {
-	wind[0] = _state.wind_vel(0);
-	wind[1] = _state.wind_vel(1);
+	return _state.wind_vel;
 }
 
-void Ekf::get_wind_velocity_var(float *wind_var)
+Vector2f Ekf::getWindVelocityVariance() const
 {
-	wind_var[0] = P(22,22);
-	wind_var[1] = P(23,23);
+	return P.slice<2, 2>(22,22).diag();
 }
 
 void Ekf::get_true_airspeed(float *tas)
@@ -248,8 +246,7 @@ void Ekf::get_true_airspeed(float *tas)
 */
 void Ekf::resetWindStates()
 {
-	// get euler yaw angle
-	Eulerf euler321(_state.quat_nominal);
+	const Eulerf euler321(_state.quat_nominal);
 	const float euler_yaw = euler321(2);
 
 	if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {

EKF/covariance.cpp

@@ -101,15 +101,14 @@ void Ekf::initialiseCovariance()
 
 }
 
-void Ekf::get_pos_var(Vector3f &pos_var)
+Vector3f Ekf::getPositionVariance() const
 {
-	pos_var = P.slice<3,3>(7,7).diag();
+	return P.slice<3,3>(7,7).diag();
 }
 
-void Ekf::get_vel_var(Vector3f &vel_var)
+Vector3f Ekf::getVelocityVariance() const
 {
-	vel_var = P.slice<3,3>(4,4).diag();
-
+	return P.slice<3,3>(4,4).diag();
 }
 
 void Ekf::predictCovariance()

EKF/ekf.cpp

@@ -350,17 +350,17 @@ void Ekf::calculateOutputStates()
 	_R_to_earth_now = Dcmf(_output_new.quat_nominal);
 
 	// correct delta velocity for bias offsets
-	const Vector3f delta_vel{imu.delta_vel - _state.delta_vel_bias * dt_scale_correction};
+	const Vector3f delta_vel_body{imu.delta_vel - _state.delta_vel_bias * dt_scale_correction};
 
 	// rotate the delta velocity to earth frame
-	Vector3f delta_vel_NED{_R_to_earth_now * delta_vel};
+	Vector3f delta_vel_earth{_R_to_earth_now * delta_vel_body};
 
 	// correct for measured acceleration due to gravity
-	delta_vel_NED(2) += CONSTANTS_ONE_G * imu.delta_vel_dt;
+	delta_vel_earth(2) += CONSTANTS_ONE_G * imu.delta_vel_dt;
 
 	// calculate the earth frame velocity derivatives
 	if (imu.delta_vel_dt > 1e-4f) {
-		_vel_deriv_ned = delta_vel_NED * (1.0f / imu.delta_vel_dt);
+		_vel_deriv = delta_vel_earth * (1.0f / imu.delta_vel_dt);
 	}
 
 	// save the previous velocity so we can use trapezoidal integration
@@ -368,8 +368,8 @@ void Ekf::calculateOutputStates()
 
 	// increment the INS velocity states by the measurement plus corrections
 	// do the same for vertical state used by alternative correction algorithm
-	_output_new.vel += delta_vel_NED;
-	_output_vert_new.vel_d += delta_vel_NED(2);
+	_output_new.vel += delta_vel_earth;
+	_output_vert_new.vel_d += delta_vel_earth(2);
 
 	// use trapezoidal integration to calculate the INS position states
 	// do the same for vertical state used by alternative correction algorithm
@@ -427,13 +427,13 @@ void Ekf::calculateOutputStates()
 		_delta_angle_corr = delta_ang_error * att_gain;
 
 		// calculate velocity and position tracking errors
-		const Vector3f vel_err{_state.vel - _output_sample_delayed.vel};
-		const Vector3f pos_err{_state.pos - _output_sample_delayed.pos};
+		const Vector3f vel_err(_state.vel - _output_sample_delayed.vel);
+		const Vector3f pos_err(_state.pos - _output_sample_delayed.pos);
 
 		// collect magnitude tracking error for diagnostics
-		_output_tracking_error[0] = delta_ang_error.norm();
-		_output_tracking_error[1] = vel_err.norm();
-		_output_tracking_error[2] = pos_err.norm();
+		_output_tracking_error(0) = delta_ang_error.norm();
+		_output_tracking_error(1) = vel_err.norm();
+		_output_tracking_error(2) = pos_err.norm();
 
 		/*
 		 * Loop through the output filter state history and apply the corrections to the velocity and position states.

EKF/ekf.h

@@ -135,13 +135,13 @@ public:
 	void getHaglInnovRatio(float &hagl_innov_ratio) const override;
 
 	// get the state vector at the delayed time horizon
-	void get_state_delayed(float *state) override;
+	matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const override;
 
 	// get the wind velocity in m/s
-	void get_wind_velocity(float *wind) override;
+	Vector2f getWindVelocity() const override;
 
 	// get the wind velocity var
-	void get_wind_velocity_var(float *wind_var) override;
+	Vector2f getWindVelocityVariance() const override;
 
 	// get the true airspeed in m/s
 	void get_true_airspeed(float *tas) override;
@@ -187,13 +187,13 @@ public:
 	*/
 	bool reset_imu_bias() override;
 
-	void get_vel_var(Vector3f &vel_var) override;
+	Vector3f getVelocityVariance() const override;
 
-	void get_pos_var(Vector3f &pos_var) override;
+	Vector3f getPositionVariance() const override;
 
 	// return an array containing the output predictor angular, velocity and position tracking
 	// error magnitudes (rad), (m/sec), (m)
-	void get_output_tracking_error(float error[3]) override;
+	Vector3f getOutputTrackingError() const override;
 
 	/*
 	Returns  following IMU vibration metrics in the following array locations
@@ -201,7 +201,7 @@ public:
 	1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
 	2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
 	*/
-	void get_imu_vibe_metrics(float vibe[3]) override;
+	Vector3f getImuVibrationMetrics() const override;
 
 	/*
 	First argument returns GPS drift  metrics in the following array locations
@@ -224,16 +224,16 @@ public:
 	void updateTerrainValidity();
 
 	// get the estimated terrain vertical position relative to the NED origin
-	void getTerrainVertPos(float *ret) override;
+	float getTerrainVertPos() const override;
 
 	// get the terrain variance
 	float get_terrain_var() const { return _terrain_var; }
 
-	// get the accelerometer bias in m/s/s
-	void get_accel_bias(float bias[3]) override;
+	// get the accelerometer bias in m/s**2
+	Vector3f getAccelBias() const override;
 
 	// get the gyroscope bias in rad/s
-	void get_gyro_bias(float bias[3]) override;
+	Vector3f getGyroBias() const override;
 
 	// get GPS check status
 	void get_gps_check_status(uint16_t *val) override;
@@ -276,7 +276,7 @@ public:
 	void get_ekf_soln_status(uint16_t *status) override;
 
 	// return the quaternion defining the rotation from the External Vision to the EKF reference frame
-	void get_ev2ekf_quaternion(float *quat) override;
+	matrix::Quatf getVisionAlignmentQuaternion() const override;
 
 	// use the latest IMU data at the current time horizon.
 	Quatf calculate_quaternion() const;
@@ -441,7 +441,7 @@ private:
 	Vector3f _delta_angle_corr;	///< delta angle correction vector (rad)
 	Vector3f _vel_err_integ;	///< integral of velocity tracking error (m)
 	Vector3f _pos_err_integ;	///< integral of position tracking error (m.s)
-	float _output_tracking_error[3] {}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
+	Vector3f _output_tracking_error; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
 
 	// variables used for the GPS quality checks
 	Vector3f _gps_pos_deriv_filt;	///< GPS NED position derivative (m/sec)

EKF/ekf_helper.cpp

@@ -876,59 +876,51 @@ void Ekf::get_gps_check_status(uint16_t *val)
 }
 
 // get the state vector at the delayed time horizon
-void Ekf::get_state_delayed(float *state)
+matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
 {
+	matrix::Vector<float, 24> state;
 	for (int i = 0; i < 4; i++) {
-		state[i] = _state.quat_nominal(i);
+		state(i) = _state.quat_nominal(i);
 	}
 
 	for (int i = 0; i < 3; i++) {
-		state[i + 4] = _state.vel(i);
+		state(i + 4) = _state.vel(i);
 	}
 
 	for (int i = 0; i < 3; i++) {
-		state[i + 7] = _state.pos(i);
+		state(i + 7) = _state.pos(i);
 	}
 
 	for (int i = 0; i < 3; i++) {
-		state[i + 10] = _state.delta_ang_bias(i);
+		state(i + 10) = _state.delta_ang_bias(i);
 	}
 
 	for (int i = 0; i < 3; i++) {
-		state[i + 13] = _state.delta_vel_bias(i);
+		state(i + 13) = _state.delta_vel_bias(i);
 	}
 
 	for (int i = 0; i < 3; i++) {
-		state[i + 16] = _state.mag_I(i);
+		state(i + 16) = _state.mag_I(i);
 	}
 
 	for (int i = 0; i < 3; i++) {
-		state[i + 19] = _state.mag_B(i);
+		state(i + 19) = _state.mag_B(i);
 	}
 
 	for (int i = 0; i < 2; i++) {
-		state[i + 22] = _state.wind_vel(i);
+		state(i + 22) = _state.wind_vel(i);
 	}
+	return state;
 }
 
-// get the accelerometer bias
-void Ekf::get_accel_bias(float bias[3])
+Vector3f Ekf::getAccelBias() const
 {
-	float temp[3];
-	temp[0] = _state.delta_vel_bias(0) / _dt_ekf_avg;
-	temp[1] = _state.delta_vel_bias(1) / _dt_ekf_avg;
-	temp[2] = _state.delta_vel_bias(2) / _dt_ekf_avg;
-	memcpy(bias, temp, 3 * sizeof(float));
+	return _state.delta_vel_bias / _dt_ekf_avg;
 }
 
-// get the gyroscope bias in rad/s
-void Ekf::get_gyro_bias(float bias[3])
+Vector3f Ekf::getGyroBias() const
 {
-	float temp[3];
-	temp[0] = _state.delta_ang_bias(0) / _dt_ekf_avg;
-	temp[1] = _state.delta_ang_bias(1) / _dt_ekf_avg;
-	temp[2] = _state.delta_ang_bias(2) / _dt_ekf_avg;
-	memcpy(bias, temp, 3 * sizeof(float));
+	return _state.delta_ang_bias / _dt_ekf_avg;
 }
 
 // get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set
@@ -941,11 +933,11 @@ bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *orig
 	return _NED_origin_initialised;
 }
 
-// return an array containing the output predictor angular, velocity and position tracking
+// return a vector containing the output predictor angular, velocity and position tracking
 // error magnitudes (rad), (m/s), (m)
-void Ekf::get_output_tracking_error(float error[3])
+Vector3f Ekf::getOutputTrackingError() const
 {
-	memcpy(error, _output_tracking_error, 3 * sizeof(float));
+	return _output_tracking_error;
 }
 
 /*
@@ -954,9 +946,9 @@ Returns  following IMU vibration metrics in the following array locations
 1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
 2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
 */
-void Ekf::get_imu_vibe_metrics(float vibe[3])
+Vector3f Ekf::getImuVibrationMetrics() const
 {
-	memcpy(vibe, _vibe_metrics, 3 * sizeof(float));
+	return _vibe_metrics;
 }
 
 /*
@@ -1503,13 +1495,9 @@ void Ekf::calcExtVisRotMat()
 }
 
 // return the quaternions for the rotation from External Vision system reference frame to the EKF reference frame
-void Ekf::get_ev2ekf_quaternion(float *quat)
+matrix::Quatf Ekf::getVisionAlignmentQuaternion() const
 {
-	const Quatf quat_ev2ekf(_R_ev_to_ekf);
-
-	for (unsigned i = 0; i < 4; i++) {
-		quat[i] = quat_ev2ekf(i);
-	}
+	return Quatf(_R_ev_to_ekf);
 }
 
 // Increase the yaw error variance of the quaternions

EKF/estimator_interface.cpp

@@ -91,25 +91,25 @@ void EstimatorInterface::computeVibrationMetric()
 {
 	// calculate a metric which indicates the amount of coning vibration
 	Vector3f temp = _newest_high_rate_imu_sample.delta_ang % _delta_ang_prev;
-	_vibe_metrics[0] = 0.99f * _vibe_metrics[0] + 0.01f * temp.norm();
+	_vibe_metrics(0) = 0.99f * _vibe_metrics(0) + 0.01f * temp.norm();
 
 	// calculate a metric which indicates the amount of high frequency gyro vibration
 	temp = _newest_high_rate_imu_sample.delta_ang - _delta_ang_prev;
 	_delta_ang_prev = _newest_high_rate_imu_sample.delta_ang;
-	_vibe_metrics[1] = 0.99f * _vibe_metrics[1] + 0.01f * temp.norm();
+	_vibe_metrics(1) = 0.99f * _vibe_metrics(1) + 0.01f * temp.norm();
 
 	// calculate a metric which indicates the amount of high frequency accelerometer vibration
 	temp = _newest_high_rate_imu_sample.delta_vel - _delta_vel_prev;
 	_delta_vel_prev = _newest_high_rate_imu_sample.delta_vel;
-	_vibe_metrics[2] = 0.99f * _vibe_metrics[2] + 0.01f * temp.norm();
+	_vibe_metrics(2) = 0.99f * _vibe_metrics(2) + 0.01f * temp.norm();
 }
 
 bool EstimatorInterface::checkIfVehicleAtRest(float dt)
 {
 	// detect if the vehicle is not moving when on ground
 	if (!_control_status.flags.in_air) {
-		if ((_vibe_metrics[1] * 4.0E4f > _params.is_moving_scaler)
-				|| (_vibe_metrics[2] * 2.1E2f > _params.is_moving_scaler)
+		if ((_vibe_metrics(1) * 4.0E4f > _params.is_moving_scaler)
+				|| (_vibe_metrics(2) * 2.1E2f > _params.is_moving_scaler)
 				|| ((_newest_high_rate_imu_sample.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) {
 
 			_time_last_move_detect_us = _newest_high_rate_imu_sample.time_us;

EKF/estimator_interface.h

@@ -116,24 +116,17 @@ public:
 	virtual void getHaglInnovVar(float &hagl_innov_var) const = 0;
 	virtual void getHaglInnovRatio(float &hagl_innov_ratio) const = 0;
 
-	virtual void get_state_delayed(float *state) = 0;
+	virtual matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const = 0;
 
-	virtual void get_wind_velocity(float *wind) = 0;
+	virtual Vector2f getWindVelocity() const = 0;
 
-	virtual void get_wind_velocity_var(float *wind_var) = 0;
+	virtual Vector2f getWindVelocityVariance() const = 0;
 
 	virtual void get_true_airspeed(float *tas) = 0;
 
-	// gets the variances for the NED velocity states
-	virtual void get_vel_var(Vector3f &vel_var) = 0;
-
-	// gets the variances for the NED position states
-	virtual void get_pos_var(Vector3f &pos_var) = 0;
-
-
 	// return an array containing the output predictor angular, velocity and position tracking
 	// error magnitudes (rad), (m/s), (m)
-	virtual void get_output_tracking_error(float error[3]) = 0;
+	virtual Vector3f getOutputTrackingError() const = 0;
 
 	/*
 	Returns  following IMU vibration metrics in the following array locations
@@ -141,7 +134,7 @@ public:
 	1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
 	2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
 	*/
-	virtual void get_imu_vibe_metrics(float vibe[3]) = 0;
+	virtual Vector3f getImuVibrationMetrics() const = 0;
 
 	/*
 	First argument returns GPS drift  metrics in the following array locations
@@ -281,59 +274,48 @@ public:
 	bool get_terrain_valid() { return isTerrainEstimateValid(); }
 
 	// get the estimated terrain vertical position relative to the NED origin
-	virtual void getTerrainVertPos(float *ret) = 0;
-	//[[deprecated("Replaced by getTerrainVertPos")]]
-	void get_terrain_vert_pos(float *ret) { getTerrainVertPos(ret); }
+	virtual float getTerrainVertPos() const = 0;
 
 	// return true if the local position estimate is valid
 	bool local_position_is_valid();
 
-	const matrix::Quatf &get_quaternion() const { return _output_new.quat_nominal; }
+	const matrix::Quatf getQuaternion() const { return _output_new.quat_nominal; }
 
 	// return the quaternion defining the rotation from the EKF to the External Vision reference frame
-	virtual void get_ev2ekf_quaternion(float *quat) = 0;
+	virtual matrix::Quatf getVisionAlignmentQuaternion() const = 0;
 
 	// get the velocity of the body frame origin in local NED earth frame
-	void get_velocity(float *vel)
+	Vector3f getVelocity() const
 	{
-		Vector3f vel_earth = _output_new.vel - _vel_imu_rel_body_ned;
-
-		for (unsigned i = 0; i < 3; i++) {
-			vel[i] = vel_earth(i);
-		}
+		const Vector3f vel_earth = _output_new.vel - _vel_imu_rel_body_ned;
+		return vel_earth;
 	}
 
-	// get the NED velocity derivative in earth frame
-	void get_vel_deriv_ned(float *vel_deriv)
+	virtual Vector3f getVelocityVariance() const = 0;
+
+	// get the velocity derivative in earth frame
+	Vector3f getVelocityDerivative() const
 	{
-		for (unsigned i = 0; i < 3; i++) {
-			vel_deriv[i] = _vel_deriv_ned(i);
-		}
+		return _vel_deriv;
 	}
 
 	// get the derivative of the vertical position of the body frame origin in local NED earth frame
-	void get_pos_d_deriv(float *pos_d_deriv)
+	float getVerticalPositionDerivative() const
 	{
-		float var = _output_vert_new.vel_d - _vel_imu_rel_body_ned(2);
-		*pos_d_deriv = var;
+		return _output_vert_new.vel_d - _vel_imu_rel_body_ned(2);
 	}
 
-	// get the position of the body frame origin in local NED earth frame
-	void get_position(float *pos)
+	// get the position of the body frame origin in local earth frame
+	Vector3f getPosition() const
 	{
 		// rotate the position of the IMU relative to the boy origin into earth frame
-		Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body;
-
+		const Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body;
 		// subtract from the EKF position (which is at the IMU) to get position at the body origin
-		for (unsigned i = 0; i < 3; i++) {
-			pos[i] = _output_new.pos(i) - pos_offset_earth(i);
-		}
-	}
-	void copy_timestamp(uint64_t *time_us)
-	{
-		*time_us = _time_last_imu;
+		return _output_new.pos - pos_offset_earth;
 	}
 
+	virtual Vector3f getPositionVariance() const = 0;
+
 	// Get the value of magnetic declination in degrees to be saved for use at the next startup
 	// Returns true when the declination can be saved
 	// At the next startup, set param.mag_declination_deg to the value saved
@@ -349,8 +331,8 @@ public:
 		}
 	}
 
-	virtual void get_accel_bias(float bias[3]) = 0;
-	virtual void get_gyro_bias(float bias[3]) = 0;
+	virtual Vector3f getAccelBias() const = 0;
+	virtual Vector3f getGyroBias() const = 0;
 
 	// get EKF mode status
 	void get_control_mode(uint32_t *val)
@@ -482,7 +464,7 @@ protected:
 	imuSample _newest_high_rate_imu_sample{};		// imu sample capturing the newest imu data
 	Matrix3f _R_to_earth_now;		// rotation matrix from body to earth frame at current time
 	Vector3f _vel_imu_rel_body_ned;		// velocity of IMU relative to body origin in NED earth frame
-	Vector3f _vel_deriv_ned;		// velocity derivative at the IMU in NED earth frame (m/s/s)
+	Vector3f _vel_deriv;		// velocity derivative at the IMU in NED earth frame (m/s/s)
 
 	bool _imu_updated{false};      // true if the ekf should update (completed downsampling process)
 	bool _initialised{false};      // true if the ekf interface instance (data buffering) is initialized
@@ -520,7 +502,7 @@ protected:
 	// IMU vibration and movement monitoring
 	Vector3f _delta_ang_prev;	// delta angle from the previous IMU measurement
 	Vector3f _delta_vel_prev;	// delta velocity from the previous IMU measurement
-	float _vibe_metrics[3] {};	// IMU vibration metrics
+	Vector3f _vibe_metrics;	// IMU vibration metrics
 					// [0] Level of coning vibration in the IMU delta angles (rad^2)
 					// [1] high frequency vibration level in the IMU delta angle data (rad)
 					// [2] high frequency vibration level in the IMU delta velocity data (m/s)

EKF/terrain_estimator.cpp

@@ -307,7 +307,7 @@ void Ekf::updateTerrainValidity()
 }
 
 // get the estimated vertical position of the terrain relative to the NED origin
-void Ekf::getTerrainVertPos(float *ret)
+float Ekf::getTerrainVertPos() const
 {
-	memcpy(ret, &_terrain_vpos, sizeof(float));
+	return _terrain_vpos;
 }

test/sensor_simulator/ekf_logger.cpp

@@ -58,7 +58,7 @@ void EkfLogger::writeState()
 		_file << time;
 		if(_state_logging_enabled)
 		{
-			matrix::Vector<float, 24> state = _ekf_wrapper.getState();
+			matrix::Vector<float, 24> state = _ekf->getStateAtFusionHorizonAsVector();
 			for(int i = 0; i < 24; i++)
 			{
 				_file << "," << state(i);

test/sensor_simulator/ekf_wrapper.cpp

@@ -160,74 +160,12 @@ bool EkfWrapper::isWindVelocityEstimated() const
 	return control_status.flags.wind;
 }
 
-Vector3f EkfWrapper::getPosition() const
-{
-	float temp[3];
-	_ekf->get_position(temp);
-	return Vector3f(temp);
-}
-Vector3f EkfWrapper::getVelocity() const
-{
-	float temp[3];
-	_ekf->get_velocity(temp);
-	return Vector3f(temp);
-}
-Vector3f EkfWrapper::getAccelBias() const
-{
-	float temp[3];
-	_ekf->get_accel_bias(temp);
-	return Vector3f(temp);
-}
-
-Vector3f EkfWrapper::getGyroBias() const
-{
-	float temp[3];
-	_ekf->get_gyro_bias(temp);
-	return Vector3f(temp);
-}
-
-Quatf EkfWrapper::getQuaternion() const
-{
-	return _ekf->get_quaternion();
-}
-
 Eulerf EkfWrapper::getEulerAngles() const
 {
-	return Eulerf(getQuaternion());
-}
-
-matrix::Vector<float, 24> EkfWrapper::getState() const
-{
-	float state[24];
-	_ekf->get_state_delayed(state);
-	return matrix::Vector<float, 24>{state};
+	return Eulerf(_ekf->getQuaternion());
 }
 
 matrix::Vector<float, 4> EkfWrapper::getQuaternionVariance() const
 {
 	return matrix::Vector<float, 4>(_ekf->orientation_covariances().diag());
 }
-
-Vector3f EkfWrapper::getPositionVariance() const
-{
-	return Vector3f(_ekf->position_covariances().diag());
-}
-
-Vector3f EkfWrapper::getVelocityVariance() const
-{
-	return Vector3f(_ekf->velocity_covariances().diag());
-}
-
-Vector2f EkfWrapper::getWindVelocity() const
-{
-	float temp[2];
-	_ekf->get_wind_velocity(temp);
-	return Vector2f(temp);
-}
-
-Quatf EkfWrapper::getVisionAlignmentQuaternion() const
-{
-	float temp[4];
-	_ekf->get_ev2ekf_quaternion(temp);
-	return Quatf(temp);
-}

test/sensor_simulator/ekf_wrapper.h

@@ -85,19 +85,8 @@ public:
 
 	bool isWindVelocityEstimated() const;
 
-	Vector3f getPosition() const;
-	Vector3f getVelocity() const;
-	Vector3f getAccelBias() const;
-	Vector3f getGyroBias() const;
-	Quatf getQuaternion() const;
 	Eulerf getEulerAngles() const;
-	matrix::Vector<float, 24> getState() const;
 	matrix::Vector<float, 4> getQuaternionVariance() const;
-	Vector3f getPositionVariance() const;
-	Vector3f getVelocityVariance() const;
-	Vector2f getWindVelocity() const;
-
-	Quatf getVisionAlignmentQuaternion() const;
 
 private:
 	std::shared_ptr<Ekf> _ekf;

test/test_EKF_airspeed.cpp

@@ -88,9 +88,9 @@ TEST_F(EkfAirspeedTest, testWindVelocityEstimation)
 
 	EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated());
 
-	const Vector3f vel = _ekf_wrapper.getVelocity();
-	const Vector2f vel_wind_earth = _ekf_wrapper.getWindVelocity();
-	const float height_before_pressure_correction = _ekf_wrapper.getPosition()(2);
+	const Vector3f vel = _ekf->getVelocity();
+	const Vector2f vel_wind_earth = _ekf->getWindVelocity();
+	const float height_before_pressure_correction = _ekf->getPosition()(2);
 
 	const Dcmf R_to_earth_sim(_quat_sim);
 	EXPECT_TRUE(matrix::isEqual(vel, simulated_velocity_earth));
@@ -108,7 +108,7 @@ TEST_F(EkfAirspeedTest, testWindVelocityEstimation)
 
 	_sensor_simulator.runSeconds(20);
 
-	const float height_after_pressure_correction = _ekf_wrapper.getPosition()(2);
+	const float height_after_pressure_correction = _ekf->getPosition()(2);
 	// height increase means that state z decrease due to z axis pointing down
 	const float expected_height_after_pressure_correction = height_before_pressure_correction -
 								expected_height_difference;

test/test_EKF_basics.cpp

@@ -113,11 +113,11 @@ TEST_F(EkfBasicsTest, convergesToZero)
 	// GIVEN: initialized EKF with default IMU, baro and mag input
 	_sensor_simulator.runSeconds(4);
 
-	Vector3f pos = _ekf_wrapper.getPosition();
-	Vector3f vel = _ekf_wrapper.getVelocity();
-	Vector3f accel_bias = _ekf_wrapper.getAccelBias();
-	Vector3f gyro_bias = _ekf_wrapper.getGyroBias();
-	Vector3f ref{0.0f, 0.0f, 0.0f};
+	const Vector3f pos = _ekf->getPosition();
+	const Vector3f vel = _ekf->getVelocity();
+	const Vector3f accel_bias = _ekf->getAccelBias();
+	const Vector3f gyro_bias = _ekf->getGyroBias();
+	const Vector3f ref{0.0f, 0.0f, 0.0f};
 
 	// THEN: EKF should stay or converge to zero
 	EXPECT_TRUE(matrix::isEqual(pos, ref, 0.001f));
@@ -168,17 +168,17 @@ TEST_F(EkfBasicsTest, accleBiasEstimation)
 {
 	// GIVEN: initialized EKF with default IMU, baro and mag input for 3s
 	// WHEN: Added more sensor measurements with accel bias and gps measurements
-	Vector3f accel_bias_sim = {0.0f,0.0f,0.1f};
+	const Vector3f accel_bias_sim = {0.0f,0.0f,0.1f};
 
 	_sensor_simulator.startGps();
 	_sensor_simulator.setImuBias(accel_bias_sim, Vector3f{0.0f,0.0f,0.0f});
 	_sensor_simulator.runSeconds(10);
 
-	Vector3f pos = _ekf_wrapper.getPosition();
-	Vector3f vel = _ekf_wrapper.getVelocity();
-	Vector3f accel_bias = _ekf_wrapper.getAccelBias();
-	Vector3f gyro_bias = _ekf_wrapper.getGyroBias();
-	Vector3f zero{0.0f, 0.0f, 0.0f};
+	const Vector3f pos = _ekf->getPosition();
+	const Vector3f vel = _ekf->getVelocity();
+	const Vector3f accel_bias = _ekf->getAccelBias();
+	const Vector3f gyro_bias = _ekf->getGyroBias();
+	const Vector3f zero{0.0f, 0.0f, 0.0f};
 
 	// THEN: EKF should stay or converge to zero
 	EXPECT_TRUE(matrix::isEqual(pos, zero, 0.01f));

test/test_EKF_externalVision.cpp

@@ -103,7 +103,7 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
 
 TEST_F(EkfExternalVisionTest, visionVarianceCheck)
 {
-	const Vector3f velVar_init = _ekf_wrapper.getVelocityVariance();
+	const Vector3f velVar_init = _ekf->getVelocityVariance();
 	EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
 
 	_sensor_simulator._vio.setVelocityVariance(Vector3f{2.0f,0.01f,0.01f});
@@ -111,7 +111,7 @@ TEST_F(EkfExternalVisionTest, visionVarianceCheck)
 	_sensor_simulator.startExternalVision();
 	_sensor_simulator.runSeconds(4);
 
-	const Vector3f velVar_new = _ekf_wrapper.getVelocityVariance();
+	const Vector3f velVar_new = _ekf->getVelocityVariance();
 	EXPECT_TRUE(velVar_new(0) > velVar_new(1));
 }
 
@@ -131,17 +131,17 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
 	_sensor_simulator.startExternalVision();
 
-	const Vector3f velVar_init = _ekf_wrapper.getVelocityVariance();
+	const Vector3f velVar_init = _ekf->getVelocityVariance();
 	EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
 
 	_sensor_simulator.runSeconds(4);
 
 	// THEN: velocity uncertainty in y should be bigger
-	const Vector3f velVar_new = _ekf_wrapper.getVelocityVariance();
+	const Vector3f velVar_new = _ekf->getVelocityVariance();
 	EXPECT_TRUE(velVar_new(1) > velVar_new(0));
 
 	// THEN: the frame offset should be estimated correctly
-	Quatf estimatedExternalVisionFrameOffset = _ekf_wrapper.getVisionAlignmentQuaternion();
+	Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion();
 	EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(),
 				    estimatedExternalVisionFrameOffset.canonical()));
 }

test/test_EKF_fusionLogic.cpp

@@ -136,25 +136,25 @@ TEST_F(EkfFusionLogicTest, rejectGpsSignalJump)
 	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
 
 	// WHEN: Having a big horizontal position Gps jump coming from the Gps Receiver
-	Vector3f pos_old = _ekf_wrapper.getPosition();
-	Vector3f vel_old = _ekf_wrapper.getVelocity();
-	Vector3f accel_bias_old = _ekf_wrapper.getAccelBias();
+	const Vector3f pos_old = _ekf->getPosition();
+	const Vector3f vel_old = _ekf->getVelocity();
+	const Vector3f accel_bias_old = _ekf->getAccelBias();
 	_sensor_simulator._gps.stepHorizontalPositionByMeters(Vector2f{10.0f, 0.0f});
 	_sensor_simulator.runSeconds(2);
 
 	// THEN: The estimate should not change much in the short run
 	//       and GPS fusion should be stopped after a while.
-	Vector3f pos_new = _ekf_wrapper.getPosition();
-	Vector3f vel_new = _ekf_wrapper.getVelocity();
-	Vector3f accel_bias_new = _ekf_wrapper.getAccelBias();
+	Vector3f pos_new = _ekf->getPosition();
+	Vector3f vel_new = _ekf->getVelocity();
+	Vector3f accel_bias_new = _ekf->getAccelBias();
 	EXPECT_TRUE(matrix::isEqual(pos_new, pos_old, 0.01f));
 	EXPECT_TRUE(matrix::isEqual(vel_new, vel_old, 0.01f));
 	EXPECT_TRUE(matrix::isEqual(accel_bias_new, accel_bias_old, 0.01f));
 
 	_sensor_simulator.runSeconds(10);
-	pos_new = _ekf_wrapper.getPosition();
-	vel_new = _ekf_wrapper.getVelocity();
-	accel_bias_new = _ekf_wrapper.getAccelBias();
+	pos_new = _ekf->getPosition();
+	vel_new = _ekf->getVelocity();
+	accel_bias_new = _ekf->getAccelBias();
 	EXPECT_TRUE(matrix::isEqual(pos_new, pos_old, 0.01f));
 	EXPECT_TRUE(matrix::isEqual(vel_new, vel_old, 0.01f));
 	EXPECT_TRUE(matrix::isEqual(accel_bias_new, accel_bias_old, 0.01f));

test/test_EKF_initialization.cpp

@@ -64,7 +64,7 @@ class EkfInitializationTest : public ::testing::Test {
 
 	void initializedOrienationIsMatchingGroundTruth(Quatf true_quaternion)
 	{
-		Quatf quat_est = _ekf_wrapper.getQuaternion();
+		const Quatf quat_est = _ekf->getQuaternion();
 		EXPECT_TRUE(matrix::isEqual(quat_est, true_quaternion));
 	}
 
@@ -86,8 +86,8 @@ class EkfInitializationTest : public ::testing::Test {
 
 	void velocityAndPositionCloseToZero()
 	{
-		Vector3f pos = _ekf_wrapper.getPosition();
-		Vector3f vel = _ekf_wrapper.getVelocity();
+		const Vector3f pos = _ekf->getPosition();
+		const Vector3f vel = _ekf->getVelocity();
 
 		EXPECT_TRUE(matrix::isEqual(pos, Vector3f{}, 0.001f));
 		EXPECT_TRUE(matrix::isEqual(vel, Vector3f{}, 0.001f));
@@ -95,8 +95,8 @@ class EkfInitializationTest : public ::testing::Test {
 
 	void velocityAndPositionVarianceBigEnoughAfterOrientationInitialization()
 	{
-		Vector3f pos_var = _ekf_wrapper.getPositionVariance();
-		Vector3f vel_var = _ekf_wrapper.getVelocityVariance();
+		const Vector3f pos_var = _ekf->getPositionVariance();
+		const Vector3f vel_var = _ekf->getVelocityVariance();
 
 		const float pos_variance_limit = 0.2f;
 		EXPECT_TRUE(pos_var(0) > pos_variance_limit) << "pos_var(1)" << pos_var(0);