EKF: move small simple getters to header

 - return by const reference where possible

EKF/airspeed_fusion.cpp

@@ -161,11 +161,6 @@ void Ekf::fuseAirspeed()
 	}
 }
 
-Vector2f Ekf::getWindVelocity() const
-{
-	return _state.wind_vel;
-}
-
 Vector2f Ekf::getWindVelocityVariance() const
 {
 	return P.slice<2, 2>(22,22).diag();

EKF/covariance.cpp

@@ -103,16 +103,6 @@ void Ekf::initialiseCovariance()
 
 }
 
-Vector3f Ekf::getPositionVariance() const
-{
-	return P.slice<3,3>(7,7).diag();
-}
-
-Vector3f Ekf::getVelocityVariance() const
-{
-	return P.slice<3,3>(4,4).diag();
-}
-
 void Ekf::predictCovariance()
 {
 	// assign intermediate state variables

EKF/ekf.cpp

@@ -151,19 +151,21 @@ bool Ekf::initialiseFilter()
 		_accel_lpf.reset(imu_init.delta_vel / imu_init.delta_vel_dt);
 		_gyro_lpf.reset(imu_init.delta_ang / imu_init.delta_ang_dt);
 		_is_first_imu_sample = false;
+
 	} else {
 		_accel_lpf.update(imu_init.delta_vel / imu_init.delta_vel_dt);
 		_gyro_lpf.update(imu_init.delta_ang / imu_init.delta_ang_dt);
 	}
 
 	// Sum the magnetometer measurements
 	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
-		if(_mag_sample_delayed.time_us != 0)
-		{
+		if (_mag_sample_delayed.time_us != 0) {
 			_mag_counter ++;
+
 			// wait for all bad initial data to be flushed
 			if (_mag_counter <= uint8_t(_obs_buffer_length + 1)) {
 				_mag_lpf.reset(_mag_sample_delayed.mag);
+
 			} else {
 				_mag_lpf.update(_mag_sample_delayed.mag);
 			}
@@ -172,12 +174,13 @@ bool Ekf::initialiseFilter()
 
 	// accumulate enough height measurements to be confident in the quality of the data
 	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
-		if (_baro_sample_delayed.time_us != 0)
-		{
+		if (_baro_sample_delayed.time_us != 0) {
 			_baro_counter ++;
+
 			// wait for all bad initial data to be flushed
 			if (_baro_counter <= uint8_t(_obs_buffer_length + 1)) {
 				_baro_hgt_offset = _baro_sample_delayed.hgt;
+
 			} else if (_baro_counter > (uint8_t)(_obs_buffer_length + 1)) {
 				_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
 			}
@@ -190,10 +193,11 @@ bool Ekf::initialiseFilter()
 	if (not_enough_baro_samples_accumulated || not_enough_mag_samples_accumulated) {
 		return false;
 	}
+
 	// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
 	setControlBaroHeight();
 
-	if(!initialiseTilt()){
+	if (!initialiseTilt()) {
 		return false;
 	}
 
@@ -230,6 +234,7 @@ bool Ekf::initialiseTilt()
 {
 	const float accel_norm = _accel_lpf.getState().norm();
 	const float gyro_norm = _gyro_lpf.getState().norm();
+
 	if (accel_norm < 0.9f * CONSTANTS_ONE_G ||
 	    accel_norm > 1.1f * CONSTANTS_ONE_G ||
 	    gyro_norm > math::radians(15.0f)) {
@@ -394,11 +399,11 @@ void Ekf::calculateOutputStates(const imuSample &imu)
 		// this data will be at the EKF fusion time horizon
 		// TODO: there is no guarantee that data is at delayed fusion horizon
 		//       Shouldnt we use pop_first_older_than?
-		const outputSample output_delayed = _output_buffer.get_oldest();
-		const outputVert output_vert_delayed = _output_vert_buffer.get_oldest();
+		const outputSample &output_delayed = _output_buffer.get_oldest();
+		const outputVert &output_vert_delayed = _output_vert_buffer.get_oldest();
 
 		// calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon
-		const Quatf q_error( (_state.quat_nominal.inversed() * output_delayed.quat_nominal).normalized() );
+		const Quatf q_error((_state.quat_nominal.inversed() * output_delayed.quat_nominal).normalized());
 
 		// convert the quaternion delta to a delta angle
 		const float scalar = (q_error(0) >= 0.0f) ? -2.f : 2.f;

EKF/ekf.h

@@ -126,7 +126,7 @@ class Ekf final : public EstimatorInterface
 	matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const override;
 
 	// get the wind velocity in m/s
-	Vector2f getWindVelocity() const override;
+	const Vector2f &getWindVelocity() const { return _state.wind_vel; };
 
 	// get the wind velocity var
 	Vector2f getWindVelocityVariance() const override;
@@ -175,21 +175,21 @@ class Ekf final : public EstimatorInterface
 	*/
 	bool reset_imu_bias() override;
 
-	Vector3f getVelocityVariance() const override;
+	Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); };
 
-	Vector3f getPositionVariance() const override;
+	Vector3f getPositionVariance() const { return P.slice<3, 3>(7, 7).diag(); }
 
 	// return an array containing the output predictor angular, velocity and position tracking
 	// error magnitudes (rad), (m/sec), (m)
-	Vector3f getOutputTrackingError() const override { return _output_tracking_error; }
+	const Vector3f &getOutputTrackingError() const { return _output_tracking_error; }
 
 	/*
 	Returns  following IMU vibration metrics in the following array locations
 	0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
 	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)
 	*/
-	Vector3f getImuVibrationMetrics() const override { return _vibe_metrics; }
+	const Vector3f &getImuVibrationMetrics() const { return _vibe_metrics; }
 
 	/*
 	First argument returns GPS drift  metrics in the following array locations
@@ -220,10 +220,10 @@ class Ekf final : public EstimatorInterface
 	float get_terrain_var() const { return _terrain_var; }
 
 	// get the accelerometer bias in m/s**2
-	Vector3f getAccelBias() const override { return _state.delta_vel_bias / _dt_ekf_avg; }
+	Vector3f getAccelBias() const { return _state.delta_vel_bias / _dt_ekf_avg; }
 
 	// get the gyroscope bias in rad/s
-	Vector3f getGyroBias() const override { return _state.delta_ang_bias / _dt_ekf_avg; }
+	Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; }
 
 	// get GPS check status
 	void get_gps_check_status(uint16_t *val) override { *val = _gps_check_fail_status.value; }
@@ -260,13 +260,14 @@ class Ekf final : public EstimatorInterface
 	// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
 	// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
 	// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
-	void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, float &hagl, float &beta) override;
+	void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
+					float &hagl, float &beta) override;
 
 	// return a bitmask integer that describes which state estimates can be used for flight control
 	void get_ekf_soln_status(uint16_t *status) override;
 
 	// return the quaternion defining the rotation from the External Vision to the EKF reference frame
-	matrix::Quatf getVisionAlignmentQuaternion() const override;
+	matrix::Quatf getVisionAlignmentQuaternion() const { return Quatf(_R_ev_to_ekf); };
 
 	// use the latest IMU data at the current time horizon.
 	Quatf calculate_quaternion() const;
@@ -471,7 +472,7 @@ class Ekf final : public EstimatorInterface
 	gps_check_fail_status_u _gps_check_fail_status{};
 
 	// variables used to inhibit accel bias learning
-	bool _accel_bias_inhibit[3]{};		///< true when the accel bias learning is being inhibited for the specified axis
+	bool _accel_bias_inhibit[3] {};		///< true when the accel bias learning is being inhibited for the specified axis
 	Vector3f _accel_vec_filt;		///< acceleration vector after application of a low pass filter (m/sec**2)
 	float _accel_magnitude_filt{0.0f};	///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
 	float _ang_rate_magnitude_filt{0.0f};		///< angular rate magnitude after application of a decaying envelope filter (rad/sec)
@@ -541,7 +542,8 @@ class Ekf final : public EstimatorInterface
 	// variance : observaton variance
 	// gate_sigma : innovation consistency check gate size (Sigma)
 	// jacobian : 4x1 vector of partial derivatives of observation wrt each quaternion state
-	void updateQuaternion(const float innovation, const float variance, const float gate_sigma, const Vector4f &yaw_jacobian);
+	void updateQuaternion(const float innovation, const float variance, const float gate_sigma,
+			      const Vector4f &yaw_jacobian);
 
 	// fuse the yaw angle obtained from a dual antenna GPS unit
 	void fuseGpsYaw();