introduce ecl_float_t type and use in EKF

CMakeLists.txt

@@ -115,9 +115,21 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
 
 			-Wno-missing-field-initializers # ignore for GCC 4.8 support
 		)
+
+		# enable color output
+		if(CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.9)
+			# force color for gcc > 4.9
+			add_compile_options(-fdiagnostics-color=always)
+		endif()
+
+	elseif ("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang")
+		# force color for clang (needed for clang + ccache)
+		add_compile_options(-fcolor-diagnostics)
 	endif()
 
+
 	if(BUILD_TESTING)
+		enable_testing()
 
 		include(CTest)
 

EKF/CMakeLists.txt

@@ -31,6 +31,20 @@
 #
 ############################################################################
 
+if((${CONFIG_ARCH_DPFPU} MATCHES "y") AND ECL_USE_DOUBLE_FPU) # DISABLED for now
+	message(STATUS "PX4 ECL: using double precision floating point (ecl_float_t = double)")
+	add_definitions(-DECL_FLOAT_TYPE=double)
+
+	add_compile_options(
+		-Wno-double-promotion
+		-Wno-narrowing
+		-Wno-absolute-value
+	)
+else()
+	# ONLY in ecl_float_t = float32 mode
+	add_compile_options(-fsingle-precision-constant)
+endif()
+
 add_library(ecl_EKF
 	airspeed_fusion.cpp
 	control.cpp

EKF/EKFGSF_yaw.cpp

@@ -1,6 +1,8 @@
 #include "EKFGSF_yaw.h"
 #include <cstdlib>
 
+namespace estimator {
+
 EKFGSF_yaw::EKFGSF_yaw()
 {
 	// this flag must be false when we start
@@ -548,3 +550,5 @@ void EKFGSF_yaw::setVelocity(const Vector2f &velocity, float accuracy)
 	_vel_accuracy = accuracy;
 	_vel_data_updated = true;
 }
+
+} // namespace estimator

EKF/EKFGSF_yaw.h

@@ -1,29 +1,22 @@
 #pragma once
 
+#include <ecl.h>
+
 #include <geo/geo.h>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
 
 #include "common.h"
 #include "utils.hpp"
 
-using matrix::AxisAnglef;
-using matrix::Dcmf;
-using matrix::Eulerf;
-using matrix::Matrix3f;
-using matrix::Quatf;
-using matrix::Vector2f;
-using matrix::Vector3f;
-using matrix::wrap_pi;
-
 static constexpr uint8_t N_MODELS_EKFGSF = 5;
 
 // Required math constants
 static constexpr float _m_2pi_inv = 0.159154943f;
 static constexpr float _m_pi = 3.14159265f;
 static constexpr float _m_pi2 = 1.57079632f;
 
-using namespace estimator;
+namespace estimator {
 
 class EKFGSF_yaw
 {
@@ -134,3 +127,5 @@ class EKFGSF_yaw
 	// return the probability of the state estimate for the specified EKF assuming a gaussian error distribution
 	float gaussianDensity(const uint8_t model_index) const;
 };
+
+} // namespace estimator

EKF/airspeed_fusion.cpp

@@ -41,10 +41,15 @@
  * @author Paul Riseborough <[email protected]>
  *
  */
-#include "../ecl.h"
+
+#include <ecl.h>
+
 #include "ekf.h"
 #include <mathlib/mathlib.h>
 
+namespace estimator
+{
+
 void Ekf::fuseAirspeed()
 {
 	const float &vn = _state.vel(0); // Velocity in north direction
@@ -184,3 +189,5 @@ void Ekf::resetWindStates()
 		_state.wind_vel.setZero();
 	}
 }
+
+} // namespace estimator

EKF/common.h

@@ -31,6 +31,8 @@
  *
  ****************************************************************************/
 
+#pragma once
+
 /**
  * @file common.h
  * Definition of base class for attitude estimators
@@ -41,18 +43,20 @@
  */
 #pragma once
 
+#include <ecl.h>
 #include <matrix/math.hpp>
 
 namespace estimator
 {
 
-using matrix::AxisAnglef;
-using matrix::Dcmf;
-using matrix::Eulerf;
-using matrix::Matrix3f;
-using matrix::Quatf;
-using matrix::Vector2f;
-using matrix::Vector3f;
+using AxisAnglef = matrix::AxisAngle<ecl_float_t>;
+using Dcmf = matrix::Dcm<ecl_float_t>;
+using Eulerf = matrix::Euler<ecl_float_t>;
+using Matrix3f = matrix::SquareMatrix<ecl_float_t, 3>;
+using Quatf = matrix::Quaternion<ecl_float_t>;
+using Vector2f = matrix::Vector2<ecl_float_t>;
+using Vector3f = matrix::Vector3<ecl_float_t>;
+
 using matrix::wrap_pi;
 
 enum velocity_frame_t {LOCAL_FRAME_FRD, BODY_FRAME_FRD};

EKF/control.cpp

@@ -39,10 +39,14 @@
  *
  */
 
-#include "../ecl.h"
+#include <ecl.h>
+
 #include "ekf.h"
 #include <mathlib/mathlib.h>
 
+namespace estimator
+{
+
 void Ekf::controlFusionModes()
 {
 	// Store the status to enable change detection
@@ -1317,3 +1321,5 @@ void Ekf::controlAuxVelFusion()
 
 	}
 }
+
+} // namespace estimator

EKF/covariance.cpp

@@ -48,6 +48,9 @@
 #include <math.h>
 #include <mathlib/mathlib.h>
 
+namespace estimator
+{
+
 // Sets initial values for the covariance matrix
 // Do not call before quaternion states have been initialised
 void Ekf::initialiseCovariance()
@@ -57,7 +60,7 @@ void Ekf::initialiseCovariance()
 	_delta_angle_bias_var_accum.setZero();
 	_delta_vel_bias_var_accum.setZero();
 
-	const float dt = FILTER_UPDATE_PERIOD_S;
+	const ecl_float_t dt = FILTER_UPDATE_PERIOD_S;
 
 	resetQuatCov();
 
@@ -106,41 +109,41 @@ void Ekf::initialiseCovariance()
 void Ekf::predictCovariance()
 {
 	// assign intermediate state variables
-	const float &q0 = _state.quat_nominal(0);
-	const float &q1 = _state.quat_nominal(1);
-	const float &q2 = _state.quat_nominal(2);
-	const float &q3 = _state.quat_nominal(3);
+	const auto &q0 = _state.quat_nominal(0);
+	const auto &q1 = _state.quat_nominal(1);
+	const auto &q2 = _state.quat_nominal(2);
+	const auto &q3 = _state.quat_nominal(3);
 
-	const float &dax = _imu_sample_delayed.delta_ang(0);
-	const float &day = _imu_sample_delayed.delta_ang(1);
-	const float &daz = _imu_sample_delayed.delta_ang(2);
+	const auto &dax = _imu_sample_delayed.delta_ang(0);
+	const auto &day = _imu_sample_delayed.delta_ang(1);
+	const auto &daz = _imu_sample_delayed.delta_ang(2);
 
-	const float &dvx = _imu_sample_delayed.delta_vel(0);
-	const float &dvy = _imu_sample_delayed.delta_vel(1);
-	const float &dvz = _imu_sample_delayed.delta_vel(2);
+	const auto &dvx = _imu_sample_delayed.delta_vel(0);
+	const auto &dvy = _imu_sample_delayed.delta_vel(1);
+	const auto &dvz = _imu_sample_delayed.delta_vel(2);
 
-	const float &dax_b = _state.delta_ang_bias(0);
-	const float &day_b = _state.delta_ang_bias(1);
-	const float &daz_b = _state.delta_ang_bias(2);
+	const auto &dax_b = _state.delta_ang_bias(0);
+	const auto &day_b = _state.delta_ang_bias(1);
+	const auto &daz_b = _state.delta_ang_bias(2);
 
-	const float &dvx_b = _state.delta_vel_bias(0);
-	const float &dvy_b = _state.delta_vel_bias(1);
-	const float &dvz_b = _state.delta_vel_bias(2);
+	const auto &dvx_b = _state.delta_vel_bias(0);
+	const auto &dvy_b = _state.delta_vel_bias(1);
+	const auto &dvz_b = _state.delta_vel_bias(2);
 
 	// Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values
-	const float dt = FILTER_UPDATE_PERIOD_S;
-	const float dt_inv = 1.0f / dt;
+	const auto dt = FILTER_UPDATE_PERIOD_S;
+	const auto dt_inv = 1.0 / dt;
 
 	// convert rate of change of rate gyro bias (rad/s**2) as specified by the parameter to an expected change in delta angle (rad) since the last update
-	const float d_ang_bias_sig = dt * dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f);
+	const ecl_float_t d_ang_bias_sig = dt * dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f);
 
 	// convert rate of change of accelerometer bias (m/s**3) as specified by the parameter to an expected change in delta velocity (m/s) since the last update
-	const float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f);
+	const ecl_float_t d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f);
 
 	// inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
 	// xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector
-	const float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
-	const float beta = 1.0f - alpha;
+	const ecl_float_t alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
+	const ecl_float_t beta = 1.0f - alpha;
 	_ang_rate_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt);
 	_accel_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_magnitude_filt);
 	_accel_vec_filt = alpha * dt_inv * _imu_sample_delayed.delta_vel + beta * _accel_vec_filt;
@@ -228,20 +231,20 @@ void Ekf::predictCovariance()
 
 	// assign IMU noise variances
 	// inputs to the system are 3 delta angles and 3 delta velocities
-	float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f);
-	const float daxVar = sq(dt * gyro_noise);
-	const float dayVar = daxVar;
-	const float dazVar = daxVar;
+	ecl_float_t gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f);
+	const ecl_float_t daxVar = sq(dt * gyro_noise);
+	const ecl_float_t dayVar = daxVar;
+	const ecl_float_t dazVar = daxVar;
 
-	float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f);
+	ecl_float_t accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f);
 
 	if (_fault_status.flags.bad_acc_vertical) {
 		// Increase accelerometer process noise if bad accel data is detected. Measurement errors due to
 		// vibration induced clipping commonly reach an equivalent 0.5g offset.
 		accel_noise = BADACC_BIAS_PNOISE;
 	}
 
-	float dvxVar, dvyVar, dvzVar;
+	ecl_float_t dvxVar, dvyVar, dvzVar;
 	dvxVar = dvyVar = dvzVar = sq(dt * accel_noise);
 
 	// Accelerometer Clipping
@@ -1120,3 +1123,5 @@ void Ekf::resetWindCovariance()
 		P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
 	}
 }
+
+} // namespace estimator

EKF/drag_fusion.cpp

@@ -44,6 +44,9 @@
 #include <ecl.h>
 #include <mathlib/mathlib.h>
 
+namespace estimator
+{
+
 void Ekf::fuseDrag()
 {
 	SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion;  // Observation Jacobians
@@ -268,3 +271,6 @@ void Ekf::fuseDrag()
 		}
 	}
 }
+
+} // namespace estimator
+

EKF/ekf.cpp

@@ -44,6 +44,9 @@
 #include <ecl.h>
 #include <mathlib/mathlib.h>
 
+namespace estimator
+{
+
 bool Ekf::init(uint64_t timestamp)
 {
 	bool ret = initialise_interface(timestamp);
@@ -292,11 +295,11 @@ void Ekf::predictState()
 	constrainStates();
 
 	// calculate an average filter update time
-	float input = 0.5f * (_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
+	ecl_float_t input = 0.5 * (_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.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
-	_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
+	input = math::constrain(input, 0.5 * FILTER_UPDATE_PERIOD_S, 2.0 * FILTER_UPDATE_PERIOD_S);
+	_dt_ekf_avg = 0.99 * _dt_ekf_avg + 0.01 * input;
 
 	// some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication
 	// protect angainst possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate
@@ -406,14 +409,14 @@ void Ekf::calculateOutputStates(const imuSample &imu)
 		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;
+		const ecl_float_t scalar = (q_error(0) >= 0.0f) ? -2.f : 2.f;
 
 		const Vector3f delta_ang_error{scalar * q_error(1), scalar * q_error(2), scalar * q_error(3)};
 
 		// calculate a gain that provides tight tracking of the estimator attitude states and
 		// adjust for changes in time delay to maintain consistent damping ratio of ~0.7
-		const float time_delay = fmaxf((imu.time_us - _imu_sample_delayed.time_us) * 1e-6f, _dt_imu_avg);
-		const float att_gain = 0.5f * _dt_imu_avg / time_delay;
+		const ecl_float_t time_delay = fmaxf((imu.time_us - _imu_sample_delayed.time_us) * 1e-6, _dt_imu_avg);
+		const ecl_float_t att_gain = 0.5 * _dt_imu_avg / time_delay;
 
 		// calculate a corrrection to the delta angle
 		// that will cause the INS to track the EKF quaternions
@@ -428,16 +431,16 @@ void Ekf::calculateOutputStates(const imuSample &imu)
 		 */
 
 		// Complementary filter gains
-		const float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f);
-		const float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f);
+		const ecl_float_t vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, (float)_dt_ekf_avg, 10.0f);
+		const ecl_float_t pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, (float)_dt_ekf_avg, 10.0f);
 
 		// calculate down velocity and position tracking errors
-		const float vert_vel_err = (_state.vel(2) - output_vert_delayed.vert_vel);
-		const float vert_vel_integ_err = (_state.pos(2) - output_vert_delayed.vert_vel_integ);
+		const ecl_float_t vert_vel_err = (_state.vel(2) - output_vert_delayed.vert_vel);
+		const ecl_float_t vert_vel_integ_err = (_state.pos(2) - output_vert_delayed.vert_vel_integ);
 
 		// calculate a velocity correction that will be applied to the output state history
 		// using a PD feedback tuned to a 5% overshoot
-		const float vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * vel_gain * 1.1f;
+		const ecl_float_t vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * vel_gain * 1.1f;
 
 		applyCorrectionToVerticalOutputBuffer(vert_vel_correction);
 
@@ -525,13 +528,18 @@ void Ekf::applyCorrectionToOutputBuffer(const Vector3f& vel_correction, const Ve
 /*
  * Predict the previous quaternion output state forward using the latest IMU delta angle data.
 */
-Quatf Ekf::calculate_quaternion() const
+matrix::Quaternion<float>
+Ekf::calculate_quaternion() const
 {
 	// Correct delta angle data for bias errors using bias state estimates from the EKF and also apply
 	// corrections required to track the EKF quaternion states
 	const Vector3f delta_angle{_newest_high_rate_imu_sample.delta_ang - _state.delta_ang_bias * (_dt_imu_avg / _dt_ekf_avg) + _delta_angle_corr};
 
 	// increment the quaternions using the corrected delta angle vector
 	// the quaternions must always be normalised after modification
-	return Quatf{_output_new.quat_nominal * AxisAnglef{delta_angle}}.unit();
+	const Quatf q{_output_new.quat_nominal * AxisAnglef{delta_angle}};
+	const Quatf q_norm = q.unit();
+	return matrix::Quaternion<float> {q_norm(0), q_norm(1), q_norm(2), q_norm(3)};
 }
+
+} // namespace estimator