From ec3ceae45e62fb63ba6c518a1f3fcf66ee2ea1b0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 18 Apr 2024 15:14:59 -0400 Subject: [PATCH] ekf2: move yaw_estimator and derivation to dedicated folder --- src/modules/ekf2/CMakeLists.txt | 14 +++-- src/modules/ekf2/EKF/CMakeLists.txt | 15 +++++- src/modules/ekf2/EKF/ekf.h | 2 +- src/modules/ekf2/EKF/gps_control.cpp | 4 +- .../EKF/python/ekf_derivation/derivation.py | 2 +- .../derivation_terrain_estimator.py | 6 +-- .../python/ekf_derivation/utils/__init__.py | 0 .../{ => utils}/derivation_utils.py | 0 .../ekf2/EKF/yaw_estimator/CMakeLists.txt | 39 ++++++++++++++ .../EKF/{ => yaw_estimator}/EKFGSF_yaw.cpp | 34 ++++++++---- .../ekf2/EKF/{ => yaw_estimator}/EKFGSF_yaw.h | 54 ++++++++----------- .../derivation}/derivation_yaw_estimator.py | 17 +++--- .../yaw_est_compute_measurement_update.h | 0 .../generated/yaw_est_predict_covariance.h | 0 14 files changed, 126 insertions(+), 61 deletions(-) create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/utils/__init__.py rename src/modules/ekf2/EKF/python/ekf_derivation/{ => utils}/derivation_utils.py (100%) create mode 100644 src/modules/ekf2/EKF/yaw_estimator/CMakeLists.txt rename src/modules/ekf2/EKF/{ => yaw_estimator}/EKFGSF_yaw.cpp (95%) rename src/modules/ekf2/EKF/{ => yaw_estimator}/EKFGSF_yaw.h (78%) rename src/modules/ekf2/EKF/{python/ekf_derivation => yaw_estimator/derivation}/derivation_yaw_estimator.py (90%) rename src/modules/ekf2/EKF/{python/ekf_derivation => yaw_estimator/derivation}/generated/yaw_est_compute_measurement_update.h (100%) rename src/modules/ekf2/EKF/{python/ekf_derivation => yaw_estimator/derivation}/generated/yaw_est_predict_covariance.h (100%) diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 2914f5a6b8..9d42a6e242 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/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() diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 4851bdfc75..69d4edd146 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/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) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 139214866a..b0ca385d90 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/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" diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 8e5e0bff91..d8a6b26522 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/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); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index df3947e459..452d9f565b 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/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() diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py index 2d321eb6ed..14914f30c1 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py +++ b/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, diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/utils/__init__.py b/src/modules/ekf2/EKF/python/ekf_derivation/utils/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py b/src/modules/ekf2/EKF/python/ekf_derivation/utils/derivation_utils.py similarity index 100% rename from src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py rename to src/modules/ekf2/EKF/python/ekf_derivation/utils/derivation_utils.py diff --git a/src/modules/ekf2/EKF/yaw_estimator/CMakeLists.txt b/src/modules/ekf2/EKF/yaw_estimator/CMakeLists.txt new file mode 100644 index 0000000000..1f12a2ba03 --- /dev/null +++ b/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) diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp similarity index 95% rename from src/modules/ekf2/EKF/EKFGSF_yaw.cpp rename to src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp index 697e027e6d..c8655827be 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp +++ b/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 -#include "python/ekf_derivation/generated/yaw_est_predict_covariance.h" -#include "python/ekf_derivation/generated/yaw_est_compute_measurement_update.h" +#include // CONSTANTS_ONE_G + +#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, - imu_sample.delta_vel, imu_sample.delta_vel_dt, in_air); + predictEKF(model_index, delta_ang, delta_ang_dt, delta_vel, delta_vel_dt, in_air); } } diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.h b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h similarity index 78% rename from src/modules/ekf2/EKF/EKFGSF_yaw.h rename to src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h index 7add1dedef..ec84ed06f9 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.h +++ b/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 -#include -#include - -#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; +#include +#include 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) - float vel_accuracy, // 1-sigma accuracy of velocity measurement (m/s) - bool in_air); + // vel_NE: NE velocity measurement (m/s) + // vel_accuracy: 1-sigma accuracy of velocity measurement (m/s) + 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()}; // matrix that rotates a vector from body to earth frame - Vector3f gyro_bias{}; // gyro bias learned and used by the quaternion calculation + matrix::Dcmf R{matrix::eye()}; // matrix that rotates a vector from body to earth frame + 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 P{}; // covariance matrix matrix::SquareMatrix 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, - const Vector3f &delta_vel, const float delta_vel_dt, bool in_air = false); + void predictEKF(const uint8_t model_index, const matrix::Vector3f &delta_ang, const float delta_ang_dt, + 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; }; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py b/src/modules/ekf2/EKF/yaw_estimator/derivation/derivation_yaw_estimator.py similarity index 90% rename from src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py rename to src/modules/ekf2/EKF/yaw_estimator/derivation/derivation_yaw_estimator.py index e45fed5b25..076c945fde 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py +++ b/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"]) -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_predict_covariance, output_names=["P_new"]) +derivation_utils.generate_px4_function(yaw_est_compute_measurement_update, output_names=["S_inv", "S_det_inv", "K", "P_new"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h b/src/modules/ekf2/EKF/yaw_estimator/derivation/generated/yaw_est_compute_measurement_update.h similarity index 100% rename from src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h rename to src/modules/ekf2/EKF/yaw_estimator/derivation/generated/yaw_est_compute_measurement_update.h diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h b/src/modules/ekf2/EKF/yaw_estimator/derivation/generated/yaw_est_predict_covariance.h similarity index 100% rename from src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h rename to src/modules/ekf2/EKF/yaw_estimator/derivation/generated/yaw_est_predict_covariance.h