EKF angle constants in degrees for readability (#465)

* EKF angle constants in degrees for readability

* EKF make FILTER_UPDATE_PERIOD_MS static constexpr and add FILTER_UPDATE_PERIOD_S

* EKF controlOpticalFlowFusion() use constants and update comments

* EKF controlMagFusion() use angle in degrees

* EKF move earth spin rate to geo and update usage

* EKF: Fix numerical constant error and clean up comments

Comments do not need to contain numerical values when the code makes these clear.

EKF/control.cpp

@@ -58,7 +58,7 @@ void Ekf::controlFusionModes()
 
 		// Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states
 		// and declare the tilt alignment complete
-		if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) {
+		if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(math::radians(3.0f))) {
 			_control_status.flags.tilt_align = true;
 			_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
 
@@ -346,11 +346,13 @@ void Ekf::controlOpticalFlowFusion()
 	// Check if on ground motion is un-suitable for use of optical flow
 	if (!_control_status.flags.in_air) {
 		// When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation
-		float accel_norm = _accel_vec_filt.norm();
+		const float accel_norm = _accel_vec_filt.norm();
-		bool motion_is_excessive = ((accel_norm > 14.7f) // accel greater than 1.5g
+
-					    || (accel_norm < 4.9f) // accel less than 0.5g
+		const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit
+					    || (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit
 					    || (_ang_rate_mag_filt > _flow_max_rate) // angular rate exceeds flow sensor limit
-					    || (_R_to_earth(2,2) < 0.866f)); // tilted more than 30 degrees
+					    || (_R_to_earth(2,2) < cosf(math::radians(30.0f)))); // tilted excessively
+
 		if (motion_is_excessive) {
 			_time_bad_motion_us = _imu_sample_delayed.time_us;
 
@@ -516,8 +518,7 @@ void Ekf::controlGpsFusion()
 					if (_mag_inhibit_yaw_reset_req) {
 						_mag_inhibit_yaw_reset_req = false;
 						// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
-						float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS;
+						setDiag(P, 12, 12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
-						setDiag(P, 12, 12, sq(_params.switch_on_gyro_bias * dt));
 					}
 				}
 
@@ -1307,7 +1308,7 @@ void Ekf::controlMagFusion()
 			} else if (_mag_bias_observable) {
 				// monitor yaw rotation in 45 deg sections.
 				// a rotation of 45 deg is sufficient to make the mag bias observable
-				if (fabsf(_yaw_delta_ef) > 0.7854f) {
+				if (fabsf(_yaw_delta_ef) > math::radians(45.0f)) {
 					_time_yaw_started = _imu_sample_delayed.time_us;
 					_yaw_delta_ef = 0.0f;
 				}

EKF/covariance.cpp

@@ -55,7 +55,7 @@ void Ekf::initialiseCovariance()
 	}
 
 	// calculate average prediction time step in sec
-	float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS;
+	float dt = FILTER_UPDATE_PERIOD_S;
 
 	// define the initial angle uncertainty as variances for a rotation vector
 	Vector3f rot_vec_var;
@@ -150,7 +150,7 @@ void Ekf::predictCovariance()
 	float dvy_b = _state.accel_bias(1);
 	float dvz_b = _state.accel_bias(2);
 
-	float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.0005f * FILTER_UPDATE_PERIOD_MS, 0.002f * FILTER_UPDATE_PERIOD_MS);
+	float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
 	float dt_inv = 1.0f / dt;
 
 	// compute noise variance for stationary processes
@@ -907,7 +907,7 @@ void Ekf::resetWindCovariance()
 		// calculate the uncertainty in wind speed and direction using the uncertainty in airspeed and sideslip angle
 		// used to calculate the initial wind speed
 		float R_spd = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
-		float R_yaw = sq(0.1745f);
+		float R_yaw = sq(math::radians(10.0f));
 
 		// calculate the variance and covariance terms for the wind states
 		float cos_yaw = cosf(yaw);

EKF/ekf.cpp

@@ -85,7 +85,7 @@ bool Ekf::init(uint64_t timestamp)
 	_control_status.value = 0;
 	_control_status_prev.value = 0;
 
-	_dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS);
+	_dt_ekf_avg = FILTER_UPDATE_PERIOD_S;
 
 	_fault_status.value = 0;
 	_innov_check_fail_status.value = 0;
@@ -307,7 +307,7 @@ void Ekf::predictState()
 {
 	if (!_earth_rate_initialised) {
 		if (_NED_origin_initialised) {
-			calcEarthRateNED(_earth_rate_NED, _pos_ref.lat_rad);
+			calcEarthRateNED(_earth_rate_NED, (float)_pos_ref.lat_rad);
 			_earth_rate_initialised = true;
 		}
 	}
@@ -359,7 +359,7 @@ void Ekf::predictState()
 	float input = 0.5f * (_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
 
 	// filter and limit input between -50% and +100% of nominal value
-	input = math::constrain(input, 0.0005f * (float)(FILTER_UPDATE_PERIOD_MS), 0.002f * (float)(FILTER_UPDATE_PERIOD_MS));
+	input = math::constrain(input, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
 	_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
 }
 
@@ -391,7 +391,7 @@ bool Ekf::collect_imu(const imuSample &imu)
 
 	// if the target time delta between filter prediction steps has been exceeded
 	// write the accumulated IMU data to the ring buffer
-	const float target_dt = FILTER_UPDATE_PERIOD_MS / 1000.0f;
+	const float target_dt = FILTER_UPDATE_PERIOD_S;
 
 	if (_imu_down_sampled.delta_ang_dt >= target_dt - _imu_collection_time_adj) {
 

EKF/ekf.h

@@ -237,7 +237,6 @@ public:
 private:
 
 	static constexpr uint8_t _k_num_states{24};		///< number of EKF states
-	static constexpr float _k_earth_rate{0.000072921f};	///< earth spin rate (rad/sec)
 
 	struct {
 		uint8_t velNE_counter;	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
@@ -252,7 +251,7 @@ private:
 		Quatf quat_change;	///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion
 	} _state_reset_status{};	///< reset event monitoring structure containing velocity, position, height and yaw reset information
 
-	float _dt_ekf_avg{0.001f * FILTER_UPDATE_PERIOD_MS}; ///< average update rate of the ekf
+	float _dt_ekf_avg{FILTER_UPDATE_PERIOD_S}; ///< average update rate of the ekf
 	float _dt_update{0.01f}; ///< delta time since last ekf update. This time can be used for filters which run at the same rate as the Ekf::update() function. (sec)
 
 	stateSample _state{};		///< state struct of the ekf running at the delayed time horizon
@@ -548,7 +547,7 @@ private:
 	void fuse(float *K, float innovation);
 
 	// calculate the earth rotation vector from a given latitude
-	void calcEarthRateNED(Vector3f &omega, double lat_rad) const;
+	void calcEarthRateNED(Vector3f &omega, float lat_rad) const;
 
 	// return true id the GPS quality is good enough to set an origin and start aiding
 	bool gps_is_good(struct gps_message *gps);
@@ -602,10 +601,7 @@ private:
 	void checkForStuckRange();
 
 	// return the square of two floating point numbers - used in auto coded sections
-	inline float sq(float var)
+	static constexpr float sq(float var) { return var * var; }
-	{
-		return var * var;
-	}
 
 	// set control flags to use baro height
 	void setControlBaroHeight();

EKF/ekf_helper.cpp

@@ -793,7 +793,7 @@ void Ekf::constrainStates()
 	}
 
 	for (int i = 0; i < 3; i++) {
-		_state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -0.349066f * _dt_ekf_avg, 0.349066f * _dt_ekf_avg);
+		_state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -math::radians(20.f) * _dt_ekf_avg, math::radians(20.f) * _dt_ekf_avg);
 	}
 
 	for (int i = 0; i < 3; i++) {
@@ -814,11 +814,11 @@ void Ekf::constrainStates()
 }
 
 // calculate the earth rotation vector
-void Ekf::calcEarthRateNED(Vector3f &omega, double lat_rad) const
+void Ekf::calcEarthRateNED(Vector3f &omega, float lat_rad) const
 {
-	omega(0) = _k_earth_rate * cosf((float)lat_rad);
+	omega(0) = CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad);
 	omega(1) = 0.0f;
-	omega(2) = -_k_earth_rate * sinf((float)lat_rad);
+	omega(2) = -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad);
 }
 
 // gets the innovations of velocity and position measurements
@@ -1116,7 +1116,7 @@ bool Ekf::reset_imu_bias()
 	zeroRows(P, 10, 15);
 
 	// Set the corresponding variances to the values use for initial alignment
-	float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS;
+	float dt = FILTER_UPDATE_PERIOD_S;
 	P[12][12] = P[11][11] = P[10][10] = sq(_params.switch_on_gyro_bias * dt);
 	P[15][15] = P[14][14] = P[13][13] = sq(_params.switch_on_accel_bias * dt);
 	_last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us;

EKF/estimator_interface.h

@@ -381,7 +381,8 @@ public:
 
 	void print_status();
 
-	static const unsigned FILTER_UPDATE_PERIOD_MS = 8;	// ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
+	static constexpr unsigned FILTER_UPDATE_PERIOD_MS{8};	// ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
+	static constexpr float FILTER_UPDATE_PERIOD_S{FILTER_UPDATE_PERIOD_MS * 0.001f};
 
 protected:
 
@@ -391,7 +392,7 @@ protected:
 	 OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer
 	 which sets the maximum frequency at which we can process non-IMU measurements. Measurements that
 	 arrive too soon after the previous measurement will not be processed.
-	 max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS * 0.001)
+	 max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_S)
 	 This can be adjusted to match the max sensor data rate plus some margin for jitter.
 	*/
 	uint8_t _obs_buffer_length{0};

geo/geo.h

@@ -61,6 +61,8 @@ static constexpr float CONSTANTS_ABSOLUTE_NULL_CELSIUS = -273.15f;				// °C
 static constexpr double CONSTANTS_RADIUS_OF_EARTH = 6371000;					// meters (m)
 static constexpr float  CONSTANTS_RADIUS_OF_EARTH_F = CONSTANTS_RADIUS_OF_EARTH;		// meters (m)
 
+static constexpr float CONSTANTS_EARTH_SPIN_RATE = 7.2921150e-5f;				// radians/second (rad/s)
+
 
 // XXX remove
 struct crosstrack_error_s {