From ec15fe3d9059132014087a3dc796b0750156b91c Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Wed, 11 Oct 2023 13:40:26 +0200 Subject: [PATCH] ekf2-derivation: fix terrain and yaw estimator derivations fix compatibility issues with symforce-0.9.0 --- .../python/ekf_derivation/derivation_terrain_estimator.py | 5 ++++- .../EKF/python/ekf_derivation/derivation_yaw_estimator.py | 3 +++ .../generated/terr_est_compute_flow_xy_innov_var_and_hx.h | 8 ++++---- .../generated/terr_est_compute_flow_y_innov_var_and_h.h | 8 ++++---- .../generated/yaw_est_compute_measurement_update.h | 2 +- .../ekf_derivation/generated/yaw_est_predict_covariance.h | 2 +- 6 files changed, 17 insertions(+), 11 deletions(-) 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 29f2504133..2d321eb6ed 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 @@ -33,6 +33,9 @@ File: derivation_terrain_estimator.py Description: """ +import symforce +symforce.set_epsilon_to_symbol() + import symforce.symbolic as sf from derivation_utils import * @@ -43,7 +46,7 @@ def predict_opt_flow( pos_z: sf.Scalar, epsilon : sf.Scalar ): - R_to_earth = quat_to_rot(q_att) + R_to_earth = sf.Rot3(sf.Quaternion(xyz=q_att[1:4], w=q_att[0])).to_rotation_matrix() flow_pred = sf.V2() dist = - (pos_z - terrain_vpos) dist = add_epsilon_sign(dist, dist, epsilon) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py index 1b0bdb9076..b950b54d09 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py @@ -33,6 +33,9 @@ File: derivation_yaw_estimator.py Description: """ +import symforce +symforce.set_epsilon_to_symbol() + import symforce.symbolic as sf from derivation_utils import * diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h index b2af651030..6b82a6534f 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- @@ -35,13 +35,13 @@ void TerrEstComputeFlowXyInnovVarAndHx(const Scalar terrain_vpos, const Scalar P const Scalar R, const Scalar epsilon, matrix::Matrix* const innov_var = nullptr, Scalar* const H = nullptr) { - // Total ops: 28 + // Total ops: 27 // Input arrays // Intermediate terms (4) - const Scalar _tmp0 = std::pow(q_att(0, 0), Scalar(2)) - std::pow(q_att(1, 0), Scalar(2)) - - std::pow(q_att(2, 0), Scalar(2)) + std::pow(q_att(3, 0), Scalar(2)); + const Scalar _tmp0 = + -2 * std::pow(q_att(1, 0), Scalar(2)) - 2 * std::pow(q_att(2, 0), Scalar(2)) + 1; const Scalar _tmp1 = pos_z - terrain_vpos; const Scalar _tmp2 = -_tmp1 + epsilon * (2 * math::min(0, -(((_tmp1) > 0) - ((_tmp1) < 0))) + 1); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h index 0bf7cb85f0..461db323cf 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- @@ -34,13 +34,13 @@ void TerrEstComputeFlowYInnovVarAndH(const Scalar terrain_vpos, const Scalar P, const matrix::Matrix& v, const Scalar pos_z, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, Scalar* const H = nullptr) { - // Total ops: 26 + // Total ops: 25 // Input arrays // Intermediate terms (3) - const Scalar _tmp0 = std::pow(q_att(0, 0), Scalar(2)) - std::pow(q_att(1, 0), Scalar(2)) - - std::pow(q_att(2, 0), Scalar(2)) + std::pow(q_att(3, 0), Scalar(2)); + const Scalar _tmp0 = + -2 * std::pow(q_att(1, 0), Scalar(2)) - 2 * std::pow(q_att(2, 0), Scalar(2)) + 1; const Scalar _tmp1 = pos_z - terrain_vpos; const Scalar _tmp2 = -_tmp1 + epsilon * (2 * math::min(0, -(((_tmp1) > 0) - ((_tmp1) < 0))) + 1); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h index 19d384712b..4f0048b7ac 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h index 352f9d75b0..02f48e62b7 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // -----------------------------------------------------------------------------