diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 83d12e944a..d0a796c315 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -33,38 +33,85 @@ add_subdirectory(Utility) +option(EKF2_SYMFORCE_GEN "ekf2 generate symforce output" OFF) + # Symforce code generation TODO:fixme -# execute_process( -# COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic -# RESULT_VARIABLE PYTHON_SYMFORCE_EXIT_CODE -# OUTPUT_QUIET -# ) -# if(${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0) +execute_process( + COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic + RESULT_VARIABLE PYTHON_SYMFORCE_EXIT_CODE + OUTPUT_QUIET +) -# set(EKF_DERIVATION_DIR ${CMAKE_CURRENT_SOURCE_DIR}/EKF/python/ekf_derivation) +# for now only provide symforce target helper if derivation.py generation isn't default +if((NOT CONFIG_EKF2_MAGNETOMETER) OR (NOT CONFIG_EKF2_WIND)) + set(EKF2_SYMFORCE_GEN ON) +endif() -# set(EKF_GENERATED_SRC_FILES -# ${EKF_DERIVATION_DIR}/generated/compute_airspeed_h_and_k.h -# ${EKF_DERIVATION_DIR}/generated/compute_airspeed_innov_and_innov_var.h -# ${EKF_DERIVATION_DIR}/generated/compute_sideslip_h_and_k.h -# ${EKF_DERIVATION_DIR}/generated/compute_sideslip_innov_and_innov_var.h -# ${EKF_DERIVATION_DIR}/generated/covariance_prediction.h -# ) +set(EKF_DERIVATION_SRC_DIR ${CMAKE_CURRENT_SOURCE_DIR}/EKF/python/ekf_derivation) -# add_custom_command( -# OUTPUT ${EKF_GENERATED_SRC_FILES} -# COMMAND ${PYTHON_EXECUTABLE} derivation.py -# DEPENDS -# ${EKF_DERIVATION_DIR}/derivation.py -# ${EKF_DERIVATION_DIR}/derivation_utils.py +set(EKF_GENERATED_FILES ${EKF_DERIVATION_SRC_DIR}/generated/state.h) +set(EKF_GENERATED_DERIVATION_INCLUDE_PATH "${EKF_DERIVATION_SRC_DIR}/..") -# WORKING_DIRECTORY ${EKF_DERIVATION_DIR} -# COMMENT "Symforce code generation" -# USES_TERMINAL -# ) +if(EKF2_SYMFORCE_GEN AND (${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0)) -# add_custom_target(ekf2_symforce_generate DEPENDS ${EKF_GENERATED_SRC_FILES}) -# endif() + # regenerate default in tree + add_custom_command( + OUTPUT + ${EKF_DERIVATION_SRC_DIR}/generated/predict_covariance.h + ${EKF_DERIVATION_SRC_DIR}/generated/state.h + COMMAND + ${PYTHON_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py + DEPENDS + ${EKF_DERIVATION_SRC_DIR}/derivation.py + ${EKF_DERIVATION_SRC_DIR}/derivation_utils.py + + WORKING_DIRECTORY ${EKF_DERIVATION_SRC_DIR} + COMMENT "Symforce code generation (default)" + USES_TERMINAL + ) + + # generate to build directory + set(EKF_DERIVATION_DST_DIR ${CMAKE_CURRENT_BINARY_DIR}/ekf_derivation) + file(MAKE_DIRECTORY ${EKF_DERIVATION_DST_DIR}) + + set(EKF_GENERATED_FILES ${EKF_DERIVATION_DST_DIR}/generated/state.h) + set(EKF_GENERATED_DERIVATION_INCLUDE_PATH ${CMAKE_CURRENT_BINARY_DIR}) + + set(SYMFORCE_ARGS) + + if(NOT CONFIG_EKF2_MAGNETOMETER) + message(STATUS "ekf2: symforce disabling mag") + list(APPEND SYMFORCE_ARGS "--disable_mag") + endif() + + if(NOT CONFIG_EKF2_WIND) + message(STATUS "ekf2: symforce disabling wind") + list(APPEND SYMFORCE_ARGS "--disable_wind") + endif() + + add_custom_command( + OUTPUT + ${EKF_DERIVATION_DST_DIR}/generated/predict_covariance.h + ${EKF_DERIVATION_DST_DIR}/generated/state.h + COMMAND + ${PYTHON_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py ${SYMFORCE_ARGS} + DEPENDS + ${EKF_DERIVATION_SRC_DIR}/derivation.py + ${EKF_DERIVATION_SRC_DIR}/derivation_utils.py + + WORKING_DIRECTORY ${EKF_DERIVATION_DST_DIR} + COMMENT "Symforce code generation" + USES_TERMINAL + ) + + + + add_custom_target(ekf2_symforce_generate + DEPENDS + ${EKF_DERIVATION_SRC_DIR}/generated/predict_covariance.h + ${EKF_DERIVATION_DST_DIR}/generated/predict_covariance.h + ) +endif() set(EKF_SRCS) list(APPEND EKF_SRCS @@ -165,6 +212,7 @@ px4_add_module( #-O0 INCLUDES EKF + ${EKF_GENERATED_DERIVATION_INCLUDE_PATH} PRIORITY "SCHED_PRIORITY_MAX - 18" # max priority below high priority WQ threads STACK_MAX @@ -177,6 +225,8 @@ px4_add_module( EKF2Selector.cpp EKF2Selector.hpp + ${EKF_GENERATED_FILES} + DEPENDS geo hysteresis diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 656bbcc1ce..98fb6e02ee 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -125,5 +125,6 @@ add_library(ecl_EKF ) 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_compile_options(ecl_EKF PRIVATE -fno-associative-math) diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 63e4cdee7f..f0bf822231 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -44,9 +44,9 @@ #include "ekf.h" -#include "python/ekf_derivation/generated/compute_airspeed_h_and_k.h" -#include "python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h" -#include "python/ekf_derivation/generated/compute_wind_init_and_cov_from_airspeed.h" +#include +#include +#include #include diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 50c15c8262..5960087a52 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -42,8 +42,8 @@ */ #include "ekf.h" -#include "python/ekf_derivation/generated/predict_covariance.h" -#include "python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h" +#include +#include #include #include @@ -86,7 +86,9 @@ void Ekf::initialiseCovariance() P.uncorrelateCovarianceSetVariance(State::accel_bias.idx, accel_bias_var); _prev_accel_bias_var.setAll(accel_bias_var); +#if defined(CONFIG_EKF2_MAGNETOMETER) resetMagCov(); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) // wind @@ -236,8 +238,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) } } - if (_control_status.flags.mag) { #if defined(CONFIG_EKF2_MAGNETOMETER) + if (_control_status.flags.mag) { // Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned if (P.trace(State::mag_I.idx) < 0.1f) { @@ -261,7 +263,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) nextP(i, i) += mag_B_process_noise; } } -#endif // CONFIG_EKF2_MAGNETOMETER + } else { // keep previous covariance for (unsigned i = 0; i < State::mag_I.dof; i++) { @@ -278,6 +280,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) } } } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) if (_control_status.flags.wind) { @@ -462,13 +465,13 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) } +#if defined(CONFIG_EKF2_MAGNETOMETER) // magnetic field states if (!_control_status.flags.mag) { P.uncorrelateCovarianceSetVariance(State::mag_I.idx, 0.0f); P.uncorrelateCovarianceSetVariance(State::mag_B.idx, 0.0f); } else { -#if defined(CONFIG_EKF2_MAGNETOMETER) const float mag_I_var_max = 1.f; constrainStateVar(State::mag_I, 0.f, mag_I_var_max); @@ -479,23 +482,23 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) P.makeRowColSymmetric(State::mag_I.idx); P.makeRowColSymmetric(State::mag_B.idx); } -#endif // CONFIG_EKF2_MAGNETOMETER } +#endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_WIND) // wind velocity states if (!_control_status.flags.wind) { P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, 0.0f); } else { -#if defined(CONFIG_EKF2_WIND) const float wind_vel_var_max = 1e6f; constrainStateVar(State::wind_vel, 0.f, wind_vel_var_max); if (force_symmetry) { P.makeRowColSymmetric(State::wind_vel.idx); } -#endif // CONFIG_EKF2_WIND } +#endif // CONFIG_EKF2_WIND } void Ekf::constrainStateVar(const IdxDof &state, float min, float max) @@ -543,9 +546,9 @@ void Ekf::resetQuatCov(const Vector3f &rot_var_ned) resetStateCovariance(q_cov); } +#if defined(CONFIG_EKF2_MAGNETOMETER) void Ekf::resetMagCov() { -#if defined(CONFIG_EKF2_MAGNETOMETER) if (_mag_decl_cov_reset) { ECL_INFO("reset mag covariance"); _mag_decl_cov_reset = false; @@ -555,12 +558,8 @@ void Ekf::resetMagCov() P.uncorrelateCovarianceSetVariance(State::mag_B.idx, sq(_params.mag_noise)); saveMagCovData(); -#else - P.uncorrelateCovarianceSetVariance(State::mag_I.idx, 0.f); - P.uncorrelateCovarianceSetVariance(State::mag_B.idx, 0.f); - -#endif } +#endif // CONFIG_EKF2_MAGNETOMETER void Ekf::resetGyroBiasZCov() { diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index 7ba3c9d031..ca858cd06b 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -37,8 +37,8 @@ */ #include "ekf.h" -#include "python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h" -#include "python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h" +#include +#include #include diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 276f574c09..4937e9abb8 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -68,7 +68,9 @@ void Ekf::reset() _state.mag_B.setZero(); #endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_WIND) _state.wind_vel.setZero(); +#endif // CONFIG_EKF2_WIND #if defined(CONFIG_EKF2_RANGE_FINDER) _range_sensor.setPitchOffset(_params.rng_sens_pitch); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index a6195b9810..2afa901e81 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -49,7 +49,8 @@ #include "bias_estimator.hpp" #include "height_bias_estimator.hpp" #include "position_bias_estimator.hpp" -#include "python/ekf_derivation/generated/state.h" + +#include #include #include @@ -1001,6 +1002,7 @@ private: } } +#if defined(CONFIG_EKF2_MAGNETOMETER) if (!_control_status.flags.mag) { for (unsigned i = 0; i < State::mag_I.dof; i++) { K(State::mag_I.idx + i) = 0.f; @@ -1012,12 +1014,15 @@ private: K(State::mag_B.idx + i) = 0.f; } } +#endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_WIND) if (!_control_status.flags.wind) { for (unsigned i = 0; i < State::wind_vel.dof; i++) { K(State::wind_vel.idx + i) = 0.f; } } +#endif // CONFIG_EKF2_WIND } bool measurementUpdate(VectorState &K, float innovation_variance, float innovation) @@ -1177,7 +1182,9 @@ private: void resetQuatCov(const float yaw_noise = NAN); void resetQuatCov(const Vector3f &euler_noise_ned); +#if defined(CONFIG_EKF2_MAGNETOMETER) void resetMagCov(); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) // perform a reset of the wind states and related covariances diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index faffaba236..ef6596bb00 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -40,8 +40,8 @@ */ #include "ekf.h" -#include "python/ekf_derivation/generated/quat_var_to_rot_var.h" -#include "python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h" +#include +#include #include #include @@ -244,7 +244,9 @@ void Ekf::constrainStates() _state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit()); #endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_WIND) _state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f); +#endif // CONFIG_EKF2_WIND } #if defined(CONFIG_EKF2_BARO_COMPENSATION) @@ -817,7 +819,9 @@ void Ekf::fuse(const VectorState &K, float innovation) _state.mag_B -= K.slice(State::mag_B.idx, 0) * innovation; #endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_WIND) _state.wind_vel -= K.slice(State::wind_vel.idx, 0) * innovation; +#endif // CONFIG_EKF2_WIND } void Ekf::uncorrelateQuatFromOtherStates() diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index cd2a3efd14..d02c07f994 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -41,7 +41,7 @@ */ #include "ekf.h" -#include "python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h" +#include #include #include diff --git a/src/modules/ekf2/EKF/gravity_fusion.cpp b/src/modules/ekf2/EKF/gravity_fusion.cpp index b685a92ea7..13ae1112c6 100644 --- a/src/modules/ekf2/EKF/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/gravity_fusion.cpp @@ -40,7 +40,7 @@ */ #include "ekf.h" -#include "python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h" +#include #include diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 315619ee97..03898b36d0 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -42,10 +42,12 @@ */ #include "ekf.h" -#include "python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h" -#include "python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h" -#include "python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h" -#include "python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h" + +#include +#include +#include + +#include #include diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 235a44254c..bcef15721e 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -45,8 +45,8 @@ #include #include -#include "python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h" -#include "python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h" +#include +#include void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) { diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index a5a945f98a..0981e92f0b 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -33,6 +33,8 @@ File: derivation.py Description: """ +import argparse + import symforce symforce.set_epsilon_to_symbol() @@ -42,6 +44,15 @@ from symforce import ops from symforce.values import Values from derivation_utils import * +# Initialize parser +parser = argparse.ArgumentParser() + +parser.add_argument("--disable_mag", action='store_true', help="disable mag") +parser.add_argument("--disable_wind", action='store_true', help="disable wind") + +# Read arguments from command line +args = parser.parse_args() + # The state vector is organized in an ordered dictionary State = Values( quat_nominal = sf.V4(), @@ -54,6 +65,13 @@ State = Values( wind_vel = sf.V2() ) +if args.disable_mag: + del State["mag_I"] + del State["mag_B"] + +if args.disable_wind: + del State["wind_vel"] + class IdxDof(): def __init__(self, idx, dof): self.idx = idx @@ -595,25 +613,31 @@ def rot_var_ned_to_lower_triangular_quat_cov( print("Derive EKF2 equations...") generate_px4_function(predict_covariance, output_names=["P_new"]) -generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) -generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"]) -generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"]) -generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) -generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"]) -generate_px4_function(compute_mag_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"]) -generate_px4_function(compute_mag_y_innov_var_and_h, output_names=["innov_var", "H"]) -generate_px4_function(compute_mag_z_innov_var_and_h, output_names=["innov_var", "H"]) -generate_px4_function(compute_yaw_321_innov_var_and_h, output_names=["innov_var", "H"]) -generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["innov_var", "H"]) + +if not args.disable_mag: + generate_px4_function(compute_mag_declination_pred_innov_var_and_h, output_names=["pred", "innov_var", "H"]) + generate_px4_function(compute_mag_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"]) + generate_px4_function(compute_mag_y_innov_var_and_h, output_names=["innov_var", "H"]) + generate_px4_function(compute_mag_z_innov_var_and_h, output_names=["innov_var", "H"]) + +if not args.disable_wind: + generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"]) + generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) + generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"]) + generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"]) + generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"]) + generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) + generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"]) + generate_px4_function(compute_yaw_312_innov_var_and_h, output_names=["innov_var", "H"]) generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=["innov_var", "H"]) -generate_px4_function(compute_mag_declination_pred_innov_var_and_h, output_names=["pred", "innov_var", "H"]) +generate_px4_function(compute_yaw_321_innov_var_and_h, output_names=["innov_var", "H"]) +generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["innov_var", "H"]) generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"]) generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"]) generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"]) -generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"]) -generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"]) generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"]) + generate_px4_function(quat_var_to_rot_var, output_names=["rot_var"]) generate_px4_function(rot_var_ned_to_lower_triangular_quat_cov, output_names=["q_cov_lower_triangle"]) diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index bea266d81e..939013f363 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -42,8 +42,8 @@ */ #include "ekf.h" -#include "python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h" -#include "python/ekf_derivation/generated/compute_sideslip_h_and_k.h" +#include +#include #include diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp index 770a786338..ddab6ee634 100644 --- a/src/modules/ekf2/EKF/yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -33,8 +33,8 @@ #include "ekf.h" -#include "python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h" -#include "python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h" +#include +#include #include