ekf2: move yaw_estimator and derivation to dedicated folder

src/modules/ekf2/CMakeLists.txt

@@ -63,7 +63,7 @@ if(EKF2_SYMFORCE_GEN AND (${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0))
 			${PYTHON_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py
 		DEPENDS
 			${EKF_DERIVATION_SRC_DIR}/derivation.py
-			${EKF_DERIVATION_SRC_DIR}/derivation_utils.py
+			${EKF_DERIVATION_SRC_DIR}/utils/derivation_utils.py
 
 		WORKING_DIRECTORY ${EKF_DERIVATION_SRC_DIR}
 		COMMENT "Symforce code generation (default)"
@@ -97,7 +97,7 @@ if(EKF2_SYMFORCE_GEN AND (${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0))
 			${PYTHON_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py ${SYMFORCE_ARGS}
 		DEPENDS
 			${EKF_DERIVATION_SRC_DIR}/derivation.py
-			${EKF_DERIVATION_SRC_DIR}/derivation_utils.py
+			${EKF_DERIVATION_SRC_DIR}/utils/derivation_utils.py
 
 		WORKING_DIRECTORY ${EKF_DERIVATION_DST_DIR}
 		COMMENT "Symforce code generation"
@@ -113,6 +113,7 @@ if(EKF2_SYMFORCE_GEN AND (${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0))
 	)
 endif()
 
+set(EKF_LIBS)
 set(EKF_SRCS)
 list(APPEND EKF_SRCS
 	EKF/bias_estimator.cpp
@@ -120,7 +121,6 @@ list(APPEND EKF_SRCS
 	EKF/covariance.cpp
 	EKF/ekf.cpp
 	EKF/ekf_helper.cpp
-	EKF/EKFGSF_yaw.cpp
 	EKF/estimator_interface.cpp
 	EKF/fake_height_control.cpp
 	EKF/fake_pos_control.cpp
@@ -174,6 +174,8 @@ if(CONFIG_EKF2_GNSS)
 		EKF/gps_checks.cpp
 		EKF/gps_control.cpp
 	)
+
+	list(APPEND EKF_LIBS yaw_estimator)
 endif()
 
 if(CONFIG_EKF2_GNSS_YAW)
@@ -215,6 +217,10 @@ if(CONFIG_EKF2_TERRAIN)
 	list(APPEND EKF_SRCS EKF/terrain_estimator.cpp)
 endif()
 
+add_subdirectory(EKF)
+
+
+
 px4_add_module(
 	MODULE modules__ekf2
 	MAIN ekf2
@@ -254,10 +260,10 @@ px4_add_module(
 		EKF2Utility
 		px4_work_queue
 		world_magnetic_model
+		${EKF_LIBS}
 	UNITY_BUILD
 	)
 
 if(BUILD_TESTING)
-	add_subdirectory(EKF)
 	add_subdirectory(test)
 endif()

src/modules/ekf2/EKF/CMakeLists.txt

@@ -31,6 +31,7 @@
 #
 ############################################################################
 
+set(EKF_LIBS)
 set(EKF_SRCS)
 list(APPEND EKF_SRCS
 	bias_estimator.cpp
@@ -90,8 +91,10 @@ if(CONFIG_EKF2_GNSS)
 		gnss_height_control.cpp
 		gps_checks.cpp
 		gps_control.cpp
-		EKFGSF_yaw.cpp
 	)
+
+	add_subdirectory(yaw_estimator)
+	list(APPEND EKF_LIBS yaw_estimator)
 endif()
 
 if(CONFIG_EKF2_GNSS_YAW)
@@ -133,11 +136,19 @@ if(CONFIG_EKF2_TERRAIN)
 	list(APPEND EKF_SRCS terrain_estimator.cpp)
 endif()
 
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 add_library(ecl_EKF
 	${EKF_SRCS}
 )
 
 add_dependencies(ecl_EKF prebuild_targets)
 target_include_directories(ecl_EKF PUBLIC ${EKF_GENERATED_DERIVATION_INCLUDE_PATH})
-target_link_libraries(ecl_EKF PRIVATE geo world_magnetic_model)
+
+target_link_libraries(ecl_EKF
+	PRIVATE
+		geo
+		world_magnetic_model
+		${EKF_LIBS}
+)
+
 target_compile_options(ecl_EKF PRIVATE -fno-associative-math)

src/modules/ekf2/EKF/ekf.h

@@ -46,7 +46,7 @@
 #include "estimator_interface.h"
 
 #if defined(CONFIG_EKF2_GNSS)
-# include "EKFGSF_yaw.h"
+# include "yaw_estimator/EKFGSF_yaw.h"
 #endif // CONFIG_EKF2_GNSS
 
 #include "bias_estimator.hpp"

src/modules/ekf2/EKF/gps_control.cpp

@@ -51,7 +51,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 	}
 
 	// run EKF-GSF yaw estimator once per imu_delayed update
-	_yawEstimator.predict(imu_delayed, _control_status.flags.in_air && !_control_status.flags.vehicle_at_rest);
+	_yawEstimator.predict(imu_delayed.delta_ang, imu_delayed.delta_ang_dt,
+				imu_delayed.delta_vel, imu_delayed.delta_vel_dt,
+				(_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest));
 
 	_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
 

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -48,7 +48,7 @@ from symforce import ops
 from symforce.values import Values
 
 import sympy as sp
-from derivation_utils import *
+from utils.derivation_utils import *
 
 # Initialize parser
 parser = argparse.ArgumentParser()

src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py

@@ -1,7 +1,7 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 """
-    Copyright (c) 2023 PX4 Development Team
+    Copyright (c) 2023-2024 PX4 Development Team
     Redistribution and use in source and binary forms, with or without
     modification, are permitted provided that the following conditions
     are met:
@@ -37,7 +37,7 @@ import symforce
 symforce.set_epsilon_to_symbol()
 
 import symforce.symbolic as sf
-from derivation_utils import *
+from utils.derivation_utils import *
 
 def predict_opt_flow(
         terrain_vpos: sf.Scalar,

src/modules/ekf2/EKF/yaw_estimator/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+add_library(yaw_estimator
+	EKFGSF_yaw.cpp
+	EKFGSF_yaw.h
+)
+
+add_dependencies(yaw_estimator prebuild_targets)

src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2024 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,10 +32,24 @@
  ****************************************************************************/
 
 #include "EKFGSF_yaw.h"
+
 #include <cstdlib>
 
-#include "python/ekf_derivation/generated/yaw_est_predict_covariance.h"
+#include <lib/geo/geo.h> // CONSTANTS_ONE_G
-#include "python/ekf_derivation/generated/yaw_est_compute_measurement_update.h"
+
+#include "derivation/generated/yaw_est_predict_covariance.h"
+#include "derivation/generated/yaw_est_compute_measurement_update.h"
+
+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;
+using math::Utilities::getEulerYaw;
+using math::Utilities::updateYawInRotMat;
 
 EKFGSF_yaw::EKFGSF_yaw()
 {
@@ -49,13 +63,14 @@ void EKFGSF_yaw::reset()
 	_gsf_yaw_variance = INFINITY;
 }
 
-void EKFGSF_yaw::predict(const imuSample &imu_sample, bool in_air)
+void EKFGSF_yaw::predict(const matrix::Vector3f &delta_ang, const float delta_ang_dt, const matrix::Vector3f &delta_vel,
+			 const float delta_vel_dt, bool in_air)
 {
-	const Vector3f accel = imu_sample.delta_vel / imu_sample.delta_vel_dt;
+	const Vector3f accel = delta_vel / delta_vel_dt;
 
-	if (imu_sample.delta_vel_dt > 0.001f) {
+	if (delta_vel_dt > 0.001f) {
 		// to reduce effect of vibration, filter using an LPF whose time constant is 1/10 of the AHRS tilt correction time constant
-		const float filter_coef = fminf(10.f * imu_sample.delta_vel_dt * _tilt_gain, 1.f);
+		const float filter_coef = fminf(10.f * delta_vel_dt * _tilt_gain, 1.f);
 		_ahrs_accel = _ahrs_accel * (1.f - filter_coef) + accel * filter_coef;
 
 	} else {
@@ -76,7 +91,7 @@ void EKFGSF_yaw::predict(const imuSample &imu_sample, bool in_air)
 					 && (accel_lpf_norm_sq > sq(lower_accel_limit)) && (accel_lpf_norm_sq < sq(upper_accel_limit));
 
 		if (ok_to_align) {
-			ahrsAlignTilt(imu_sample.delta_vel);
+			ahrsAlignTilt(delta_vel);
 			_ahrs_ekf_gsf_tilt_aligned = true;
 
 		} else {
@@ -85,8 +100,7 @@ void EKFGSF_yaw::predict(const imuSample &imu_sample, bool in_air)
 	}
 
 	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
-		predictEKF(model_index, imu_sample.delta_ang, imu_sample.delta_ang_dt,
+		predictEKF(model_index, delta_ang, delta_ang_dt, delta_vel, delta_vel_dt, in_air);
-			   imu_sample.delta_vel, imu_sample.delta_vel_dt, in_air);
 	}
 }
 

src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2024 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,40 +34,28 @@
 #ifndef EKF_EKFGSF_YAW_H
 #define EKF_EKFGSF_YAW_H
 
-#include <lib/geo/geo.h>
+#include <lib/mathlib/mathlib.h>
-#include <matrix/math.hpp>
+#include <lib/matrix/matrix/math.hpp>
-#include <mathlib/mathlib.h>
-
-#include "common.h"
-
-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;
 
-using namespace estimator;
-
 class EKFGSF_yaw
 {
 public:
 	EKFGSF_yaw();
 
 	// Update Filter States - this should be called whenever new IMU data is available
-	void predict(const imuSample &imu_sample, bool in_air = false);
+	void predict(const matrix::Vector3f &delta_ang, const float delta_ang_dt,
+		     const matrix::Vector3f &delta_vel, const float delta_vel_dt,
+		     bool in_air = false);
 
-	void fuseVelocity(const Vector2f &vel_NE, // NE velocity measurement (m/s)
+	// vel_NE: NE velocity measurement (m/s)
-			  float vel_accuracy,	  // 1-sigma accuracy of velocity measurement (m/s)
+	// vel_accuracy: 1-sigma accuracy of velocity measurement (m/s)
-			  bool in_air);
+	void fuseVelocity(const matrix::Vector2f &vel_NE, float vel_accuracy, bool in_air);
 
 	void setTrueAirspeed(float true_airspeed) { _true_airspeed = true_airspeed; }
 
-	void setGyroBias(const Vector3f &imu_gyro_bias, const bool force = false)
+	void setGyroBias(const matrix::Vector3f &imu_gyro_bias, const bool force = false)
 	{
 		// Initialise to gyro bias estimate from main filter because there could be a large
 		// uncorrected rate gyro bias error about the gravity vector
@@ -106,31 +94,31 @@ private:
 	float _true_airspeed{NAN};	// true airspeed used for centripetal accel compensation (m/s)
 
 	struct {
-		Dcmf R{matrix::eye<float, 3>()}; // matrix that rotates a vector from body to earth frame
+		matrix::Dcmf R{matrix::eye<float, 3>()}; // matrix that rotates a vector from body to earth frame
-		Vector3f gyro_bias{};            // gyro bias learned and used by the quaternion calculation
+		matrix::Vector3f gyro_bias{};            // gyro bias learned and used by the quaternion calculation
 	} _ahrs_ekf_gsf[N_MODELS_EKFGSF] {};
 
 	bool _ahrs_ekf_gsf_tilt_aligned{false};  // true the initial tilt alignment has been calculated
-	Vector3f _ahrs_accel{0.f, 0.f, 0.f};     // low pass filtered body frame specific force vector used by AHRS calculation (m/s/s)
+	matrix::Vector3f _ahrs_accel{0.f, 0.f, 0.f};     // low pass filtered body frame specific force vector used by AHRS calculation (m/s/s)
 
 	// calculate the gain from gravity vector misalingment to tilt correction to be used by all AHRS filters
 	float ahrsCalcAccelGain() const;
 
 	// update specified AHRS rotation matrix using IMU and optionally true airspeed data
-	void ahrsPredict(const uint8_t model_index, const Vector3f &delta_ang, const float delta_ang_dt);
+	void ahrsPredict(const uint8_t model_index, const matrix::Vector3f &delta_ang, const float delta_ang_dt);
 
 	// align all AHRS roll and pitch orientations using IMU delta velocity vector
-	void ahrsAlignTilt(const Vector3f &delta_vel);
+	void ahrsAlignTilt(const matrix::Vector3f &delta_vel);
 
 	// align all AHRS yaw orientations to initial values
 	void ahrsAlignYaw();
 
 	// Efficient propagation of a delta angle in body frame applied to the body to earth frame rotation matrix
-	Matrix3f ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g);
+	matrix::Matrix3f ahrsPredictRotMat(const matrix::Matrix3f &R, const matrix::Vector3f &g);
 
 	// Declarations used by a bank of N_MODELS_EKFGSF EKFs
 
-	struct _ekf_gsf_struct {
+	struct {
 		matrix::Vector3f X{};                       // Vel North (m/s),  Vel East (m/s), yaw (rad)s
 		matrix::SquareMatrix<float, 3> P{};         // covariance matrix
 		matrix::SquareMatrix<float, 2> S_inverse{}; // inverse of the innovation covariance matrix
@@ -141,15 +129,15 @@ private:
 	bool _ekf_gsf_vel_fuse_started{}; // true when the EKF's have started fusing velocity data and the prediction and update processing is active
 
 	// initialise states and covariance data for the GSF and EKF filters
-	void initialiseEKFGSF(const Vector2f &vel_NE, const float vel_accuracy);
+	void initialiseEKFGSF(const matrix::Vector2f &vel_NE, const float vel_accuracy);
 
 	// predict state and covariance for the specified EKF using inertial data
-	void predictEKF(const uint8_t model_index, const Vector3f &delta_ang, const float delta_ang_dt,
+	void predictEKF(const uint8_t model_index, const matrix::Vector3f &delta_ang, const float delta_ang_dt,
-			const Vector3f &delta_vel, const float delta_vel_dt, bool in_air = false);
+			const matrix::Vector3f &delta_vel, const float delta_vel_dt, bool in_air = false);
 
 	// update state and covariance for the specified EKF using a NE velocity measurement
 	// return false if update failed
-	bool updateEKF(const uint8_t model_index, const Vector2f &vel_NE, const float vel_accuracy);
+	bool updateEKF(const uint8_t model_index, const matrix::Vector2f &vel_NE, const float vel_accuracy);
 
 	inline float sq(float x) const { return x * x; };
 

src/modules/ekf2/EKF/yaw_estimator/derivation/derivation_yaw_estimator.py

@@ -1,7 +1,7 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 """
-    Copyright (c) 2022-2023 PX4 Development Team
+    Copyright (c) 2022-2024 PX4 Development Team
     Redistribution and use in source and binary forms, with or without
     modification, are permitted provided that the following conditions
     are met:
@@ -38,7 +38,12 @@ symforce.set_epsilon_to_symbol()
 
 import symforce.symbolic as sf
 from symforce.values import Values
-from derivation_utils import *
+
+# generate_px4_function from derivation_utils in EKF/ekf_derivation/utils
+import os, sys
+derivation_utils_dir = os.path.dirname(os.path.abspath(__file__)) + "/../../python/ekf_derivation/utils"
+sys.path.append(derivation_utils_dir)
+import derivation_utils
 
 State = Values(
     vel = sf.V2(),
@@ -148,7 +153,7 @@ def yaw_est_compute_measurement_update(
 
     S = H * P * H.T + R
     S_det = S[0, 0] * S[1, 1] - S[1, 0] * S[0, 1]
-    S_det_inv = add_epsilon_sign(1 / S_det, S_det, epsilon)
+    S_det_inv = derivation_utils.add_epsilon_sign(1 / S_det, S_det, epsilon)
 
     # Compute inverse using simple formula for 2x2 matrix and using protected division
     S_inv = sf.M22([[S[1, 1], -S[0, 1]], [-S[1, 0], S[0, 0]]]) * S_det_inv
@@ -166,5 +171,5 @@ def yaw_est_compute_measurement_update(
     return (S_inv, S_det_inv, K, P_new)
 
 print("Derive yaw estimator equations...")
-generate_px4_function(yaw_est_predict_covariance, output_names=["P_new"])
+derivation_utils.generate_px4_function(yaw_est_predict_covariance, output_names=["P_new"])
-generate_px4_function(yaw_est_compute_measurement_update, output_names=["S_inv", "S_det_inv", "K", "P_new"])
+derivation_utils.generate_px4_function(yaw_est_compute_measurement_update, output_names=["S_inv", "S_det_inv", "K", "P_new"])