dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2-derivation: fix terrain and yaw estimator derivations 

fix compatibility issues with symforce-0.9.0
This commit is contained in:
bresch 2023-10-11 13:40:26 +02:00 committed by Daniel Agar
parent cf1c6a8b84
commit ec15fe3d90
6 changed files with 17 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py
View File

@ -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)

3
src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py
View File

@ -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 *

8
src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h
View File

@ -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<Scalar, 2, 1>* 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<Scalar>(0, -(((_tmp1) > 0) - ((_tmp1) < 0))) + 1);

8
src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h
View File

@ -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<Scalar, 3, 1>& 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<Scalar>(0, -(((_tmp1) > 0) - ((_tmp1) < 0))) + 1);

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h
View File

@ -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.
// -----------------------------------------------------------------------------

2
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h
View File

@ -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.
// -----------------------------------------------------------------------------