From b2bcc0c9de84d3faec7551a3160ceda8008063f6 Mon Sep 17 00:00:00 2001 From: jonas Date: Sun, 20 Oct 2024 02:51:57 +0200 Subject: [PATCH] Only keep moving filter for orientation. --- .../Orientation/KF_orientation_unified.cpp | 39 ++--- .../Orientation/KF_orientation_unified.h | 46 +++-- .../Orientation/VTEOrientation.cpp | 27 ++- .../Orientation/VTEOrientation.h | 3 +- .../python_derivation/KF_unified.py | 150 ---------------- .../generated/computeInnovCov.h | 47 ----- .../python_derivation/generated/predictCov.h | 48 ------ .../generated/predictState.h | 48 ------ .../python_derivation/generated/state.h | 36 ---- .../python_derivation/generated/syncState.h | 48 ------ .../python_derivation/utils/__init__.py | 0 .../utils/derivation_utils.py | 163 ------------------ .../Position/python_derivation/KF_position.py | 2 +- 13 files changed, 62 insertions(+), 595 deletions(-) delete mode 100755 src/modules/vision_target_estimator/Orientation/python_derivation/KF_unified.py delete mode 100644 src/modules/vision_target_estimator/Orientation/python_derivation/generated/computeInnovCov.h delete mode 100644 src/modules/vision_target_estimator/Orientation/python_derivation/generated/predictCov.h delete mode 100644 src/modules/vision_target_estimator/Orientation/python_derivation/generated/predictState.h delete mode 100644 src/modules/vision_target_estimator/Orientation/python_derivation/generated/state.h delete mode 100644 src/modules/vision_target_estimator/Orientation/python_derivation/generated/syncState.h delete mode 100644 src/modules/vision_target_estimator/Orientation/python_derivation/utils/__init__.py delete mode 100644 src/modules/vision_target_estimator/Orientation/python_derivation/utils/derivation_utils.py diff --git a/src/modules/vision_target_estimator/Orientation/KF_orientation_unified.cpp b/src/modules/vision_target_estimator/Orientation/KF_orientation_unified.cpp index 889797b95043..4bb10b4afd05 100644 --- a/src/modules/vision_target_estimator/Orientation/KF_orientation_unified.cpp +++ b/src/modules/vision_target_estimator/Orientation/KF_orientation_unified.cpp @@ -34,37 +34,33 @@ // TODO: rename to KF_position and clean up comments /** - * @file KF_position_moving.cpp - * @brief Filter to estimate the pose of moving targets. State: [pos_rel, vel_uav, bias, acc_target, vel_target] + * @file KF_orientation.cpp + * @brief Filter to estimate the orientation of moving targets. State: [yaw, yaw_rate] * * @author Jonas Perolini * */ #include "KF_orientation_unified.h" -#include "python_derivation/generated/syncState.h" -#include "python_derivation/generated/predictState.h" -#include "python_derivation/generated/predictCov.h" -#include "python_derivation/generated/computeInnovCov.h" namespace vision_target_estimator { void KF_orientation_unified::predictState(float dt) { - matrix::Vector state_updated; - sym::Predictstate(dt, _state, &state_updated); - for (int i = 0; i < vtest::State::size; i++) { - _state(i) = matrix::wrap_pi(state_updated(i)); + matrix::SquareMatrix phi = getPhi(dt); + _state = phi * _state; + + for (int i = 0; i < State::size; i++) { + _state(i) = matrix::wrap_pi(_state(i)); } } void KF_orientation_unified::predictCov(float dt) { - matrix::Matrix cov_updated; - sym::Predictcov(dt, _state_covariance, &cov_updated); - _state_covariance = cov_updated; + matrix::SquareMatrix phi = getPhi(dt); + _state_covariance = phi * _state_covariance * phi.transpose(); } @@ -82,11 +78,11 @@ bool KF_orientation_unified::update() return false; } - const matrix::Matrix kalmanGain = _state_covariance * _meas_matrix_row_vect / _innov_cov; + const matrix::Matrix kalmanGain = _state_covariance * _meas_matrix_row_vect / _innov_cov; _state = _state + kalmanGain * _innov; - for (int i = 0; i < vtest::State::size; i++) { + for (int i = 0; i < 2; i++) { _state(i) = matrix::wrap_pi(_state(i)); } @@ -98,20 +94,17 @@ bool KF_orientation_unified::update() void KF_orientation_unified::syncState(float dt) { - matrix::Vector synced_state; - sym::Syncstate(dt, _state, &synced_state); + matrix::SquareMatrix phi = getPhi(dt); + _sync_state = matrix::inv(phi) * _state; - for (int i = 0; i < vtest::State::size; i++) { - _sync_state(i) = matrix::wrap_pi(synced_state(i)); + for (int i = 0; i < 2; i++) { + _sync_state(i) = matrix::wrap_pi(_sync_state(i)); } } float KF_orientation_unified::computeInnovCov(float meas_unc) { - float innov_cov_updated; - sym::Computeinnovcov(meas_unc, _state_covariance, _meas_matrix_row_vect.transpose(), &innov_cov_updated); - _innov_cov = innov_cov_updated; - + _innov_cov = (_meas_matrix_row_vect.transpose() * _state_covariance * _meas_matrix_row_vect)(0, 0) + meas_unc; return _innov_cov; } diff --git a/src/modules/vision_target_estimator/Orientation/KF_orientation_unified.h b/src/modules/vision_target_estimator/Orientation/KF_orientation_unified.h index 04e77c04c6e4..13f6161f5fe8 100644 --- a/src/modules/vision_target_estimator/Orientation/KF_orientation_unified.h +++ b/src/modules/vision_target_estimator/Orientation/KF_orientation_unified.h @@ -34,8 +34,8 @@ // TODO: rename to KF_position and clean up comments /** - * @file KF_position_moving.h - * @brief Filter to estimate the pose of moving targets. State: [r, vd, b, at, vt] + * @file KF_orientation.h + * @brief Filter to estimate the orientation of moving targets. State: [yaw, yaw_rate] * * @author Jonas Perolini * @@ -45,12 +45,19 @@ #include #include #include -#include "python_derivation/generated/state.h" #pragma once namespace vision_target_estimator { + +namespace State +{ +static constexpr uint8_t yaw{0}; +static constexpr uint8_t yaw_rate{1}; +static constexpr uint8_t size{2}; +}; + class KF_orientation_unified { public: @@ -71,20 +78,20 @@ class KF_orientation_unified // Backwards state prediciton void syncState(float dt); - void setH(const matrix::Vector &h_meas) {_meas_matrix_row_vect = h_meas;} + void setH(const matrix::Vector &h_meas) {_meas_matrix_row_vect = h_meas;} - void setState(const matrix::Vector &state) {_state = state;} + void setState(const matrix::Vector &state) {_state = state;} - void setStateVar(const matrix::Vector &var) + void setStateVar(const matrix::Vector &var) { - const matrix::SquareMatrix var_mat = diag(var); + const matrix::SquareMatrix var_mat = diag(var); _state_covariance = var_mat; }; - matrix::Vector getState() { return _state;} - matrix::Vector getStateVar() + matrix::Vector getState() { return _state;} + matrix::Vector getStateVar() { - const matrix::SquareMatrix var_mat = _state_covariance; + const matrix::SquareMatrix var_mat = _state_covariance; return var_mat.diag(); }; @@ -99,13 +106,24 @@ class KF_orientation_unified private: - matrix::Vector _state; + matrix::SquareMatrix getPhi(float dt) + { + + float data[State::size * State::size] = { + 1, dt, + 0, 1 + }; + + return matrix::SquareMatrix(data); + } + + matrix::Vector _state; - matrix::Vector _sync_state; + matrix::Vector _sync_state; - matrix::Vector _meas_matrix_row_vect; + matrix::Vector _meas_matrix_row_vect; - matrix::Matrix _state_covariance; + matrix::Matrix _state_covariance; float _innov{0.0f}; // residual of last measurement update diff --git a/src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp b/src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp index e1bd19fbd954..f6bcac19f35d 100644 --- a/src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp +++ b/src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp @@ -109,13 +109,13 @@ bool VTEOrientation::initEstimator(const float theta_init) PX4_INFO("Theta init %.2f", (double)theta_init); - matrix::Vector state_init; - // For moving targets, vtest::State::yaw_rate is init to zero, no need to explicitly set it. - state_init(vtest::State::yaw) = theta_init; + matrix::Vector state_init; + // For moving targets, State::yaw_rate is init to zero, no need to explicitly set it. + state_init(State::yaw) = theta_init; - matrix::Vector state_var_init; + matrix::Vector state_var_init; - for (int i = 0; i < vtest::State::size; i++) { + for (int i = 0; i < State::size; i++) { state_var_init(i) = _yaw_unc; } @@ -205,7 +205,7 @@ bool VTEOrientation::processObsVisionOrientation(const fiducial_marker_yaw_repor obs.updated_theta = true; obs.meas_unc_theta = vision_r_theta_unc; obs.meas_theta = vision_r_theta; - obs.meas_h_theta(vtest::State::yaw) = 1; + obs.meas_h_theta(State::yaw) = 1; return true; } @@ -272,20 +272,17 @@ void VTEOrientation::publishTarget() { vision_target_est_orientation_s vision_target_orientation{}; - matrix::Vector state = _target_estimator_orientation->getState(); - matrix::Vector state_var = _target_estimator_orientation->getStateVar(); + matrix::Vector state = _target_estimator_orientation->getState(); + matrix::Vector state_var = _target_estimator_orientation->getStateVar(); vision_target_orientation.timestamp = _last_predict; vision_target_orientation.orientation_valid = (hrt_absolute_time() - _last_update < target_valid_TIMEOUT_US); - vision_target_orientation.theta = state(vtest::State::yaw); - vision_target_orientation.cov_theta = state_var(vtest::State::yaw); + vision_target_orientation.theta = state(State::yaw); + vision_target_orientation.cov_theta = state_var(State::yaw); -#if defined(CONFIG_VTEST_MOVING) - vision_target_orientation.v_theta = state(vtest::State::yaw_rate); - vision_target_orientation.cov_v_theta = state_var(vtest::State::yaw_rate); - -#endif // CONFIG_VTEST_MOVING + vision_target_orientation.v_theta = state(State::yaw_rate); + vision_target_orientation.cov_v_theta = state_var(State::yaw_rate); _targetOrientationPub.publish(vision_target_orientation); } diff --git a/src/modules/vision_target_estimator/Orientation/VTEOrientation.h b/src/modules/vision_target_estimator/Orientation/VTEOrientation.h index bf84c750a799..e8307c52cc30 100644 --- a/src/modules/vision_target_estimator/Orientation/VTEOrientation.h +++ b/src/modules/vision_target_estimator/Orientation/VTEOrientation.h @@ -64,7 +64,6 @@ #include #include #include "KF_orientation_unified.h" -#include "python_derivation/generated/state.h" using namespace time_literals; @@ -133,7 +132,7 @@ class VTEOrientation: public ModuleParams bool updated_theta = false; float meas_theta = 0.f; float meas_unc_theta = 0.f; - matrix::Vector meas_h_theta; + matrix::Vector meas_h_theta; }; bool initTargetEstimator(); diff --git a/src/modules/vision_target_estimator/Orientation/python_derivation/KF_unified.py b/src/modules/vision_target_estimator/Orientation/python_derivation/KF_unified.py deleted file mode 100755 index 3fd7a45638bb..000000000000 --- a/src/modules/vision_target_estimator/Orientation/python_derivation/KF_unified.py +++ /dev/null @@ -1,150 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" - Copyright (c) 2022-2023 PX4 Development Team - 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. - -File: derivation.py -Description: - Derivation of an error-state EKF based on - Sola, Joan. "Quaternion kinematics for the error-state Kalman filter." arXiv preprint arXiv:1711.02508 (2017). - The derivation is directly done in discrete-time as this allows us to define the desired type of discretization - for each element while defining the equations (easier than a continuous-time derivation followed by a block-wise discretization). -""" - -import argparse - -import symforce -symforce.set_epsilon_to_symbol() - -import symforce.symbolic as sf -from symforce import typing as T -from symforce import ops -from symforce.values import Values - -import sympy as sp -from utils.derivation_utils import * - -# Initialize parser -parser = argparse.ArgumentParser() - -parser.add_argument("--moving", action='store_true', help="generate equations for moving filter") - -# Read arguments from command line -args = parser.parse_args() -moving = args.moving - -# The state vector is organized in an ordered dictionary -State = Values( - yaw=sf.V1(), - yaw_rate=sf.V1() -) - -if not moving: - del State["yaw_rate"] - -class IdxDof(): - def __init__(self, idx, dof): - self.idx = idx - self.dof = dof - -# TODO: remove dof, all dofs are one here. -def BuildTangentStateIndex(): - tangent_state_index = {} - idx = 0 - for key in State.keys_recursive(): - dof = State[key].tangent_dim() - tangent_state_index[key] = IdxDof(idx, dof) - idx += dof - return tangent_state_index - -tangent_idx = BuildTangentStateIndex() - -class VState(sf.Matrix): - SHAPE = (State.storage_dim(), 1) - -class MState(sf.Matrix): - SHAPE = (State.storage_dim(), State.storage_dim()) - -class VMeas(sf.Matrix): - SHAPE = (1, State.storage_dim()) - -def get_Phi(dt: sf.Scalar) -> sf.Matrix: - Phi = sf.Matrix.zeros(State.storage_dim(), State.storage_dim()) - - # Helper function to set blocks in the Phi matrix - def set_Phi_block(row_key, col_key, value): - idx_row = tangent_idx[row_key].idx - dof_row = tangent_idx[row_key].dof - idx_col = tangent_idx[col_key].idx - dof_col = tangent_idx[col_key].dof - if isinstance(value, sf.Matrix): - # Ensure the value matrix has the correct shape - assert value.shape == (dof_row, dof_col), "Value matrix shape mismatch" - Phi[idx_row:idx_row + dof_row, idx_col:idx_col + dof_col] = value - else: - # Scalar value; create a block matrix - block = value * sf.Matrix.eye(dof_row, dof_col) - Phi[idx_row:idx_row + dof_row, idx_col:idx_col + dof_col] = block - - # Set the diagonal elements of Phi to 1 - for key in State.keys_recursive(): - idx = tangent_idx[key].idx - dof = tangent_idx[key].dof - for i in range(dof): - Phi[idx + i, idx + i] = 1 - - if moving: - set_Phi_block('yaw', 'yaw_rate', dt) - - return Phi - -def predictState(dt: sf.Scalar, state: VState) -> VState: - Phi = get_Phi(dt) - return Phi * state - -def syncState(dt: sf.Scalar, state: VState) -> VState: - Phi = get_Phi(dt) - return Phi.inv() * (state) - -def predictCov(dt: sf.Scalar, covariance: MState) -> MState: - Phi = get_Phi(dt) - - return Phi * covariance * Phi.T - -def computeInnovCov(meas_unc: sf.Scalar, covariance: MState, meas_matrix: VMeas) -> sf.Scalar: - return (meas_matrix * covariance * meas_matrix.T)[0, 0] + meas_unc - -# Generate functions -print(f"Derive VTEST equations for {'moving' if moving else 'static'} targets...") -generate_px4_function(predictState, output_names=["predict_state"]) -generate_px4_function(syncState, output_names=["sync_state"]) -generate_px4_function(predictCov, output_names=["cov_updated"]) -generate_px4_function(computeInnovCov, output_names=["innov_cov_updated"]) - -generate_px4_state(State, tangent_idx) diff --git a/src/modules/vision_target_estimator/Orientation/python_derivation/generated/computeInnovCov.h b/src/modules/vision_target_estimator/Orientation/python_derivation/generated/computeInnovCov.h deleted file mode 100644 index d88dfe738d1a..000000000000 --- a/src/modules/vision_target_estimator/Orientation/python_derivation/generated/computeInnovCov.h +++ /dev/null @@ -1,47 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym -{ - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: computeInnovCov - * - * Args: - * meas_unc: Scalar - * covariance: Matrix11 - * meas_matrix: Matrix11 - * - * Outputs: - * innov_cov_updated: Scalar - */ -template -void Computeinnovcov(const Scalar meas_unc, const matrix::Matrix &covariance, - const matrix::Matrix &meas_matrix, - Scalar *const innov_cov_updated = nullptr) -{ - // Total ops: 3 - - // Input arrays - - // Intermediate terms (0) - - // Output terms (1) - if (innov_cov_updated != nullptr) { - Scalar &_innov_cov_updated = (*innov_cov_updated); - - _innov_cov_updated = covariance(0, 0) * std::pow(meas_matrix(0, 0), Scalar(2)) + meas_unc; - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/vision_target_estimator/Orientation/python_derivation/generated/predictCov.h b/src/modules/vision_target_estimator/Orientation/python_derivation/generated/predictCov.h deleted file mode 100644 index 684a9bacd5d5..000000000000 --- a/src/modules/vision_target_estimator/Orientation/python_derivation/generated/predictCov.h +++ /dev/null @@ -1,48 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym -{ - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: predictCov - * - * Args: - * dt: Scalar - * covariance: Matrix11 - * - * Outputs: - * cov_updated: Matrix11 - */ -template -void Predictcov(const Scalar dt, const matrix::Matrix &covariance, - matrix::Matrix *const cov_updated = nullptr) -{ - // Total ops: 0 - - // Unused inputs - (void)dt; - - // Input arrays - - // Intermediate terms (0) - - // Output terms (1) - if (cov_updated != nullptr) { - matrix::Matrix &_cov_updated = (*cov_updated); - - _cov_updated(0, 0) = covariance(0, 0); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/vision_target_estimator/Orientation/python_derivation/generated/predictState.h b/src/modules/vision_target_estimator/Orientation/python_derivation/generated/predictState.h deleted file mode 100644 index e4400f1dd09f..000000000000 --- a/src/modules/vision_target_estimator/Orientation/python_derivation/generated/predictState.h +++ /dev/null @@ -1,48 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym -{ - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: predictState - * - * Args: - * dt: Scalar - * state: Matrix11 - * - * Outputs: - * predict_state: Matrix11 - */ -template -void Predictstate(const Scalar dt, const matrix::Matrix &state, - matrix::Matrix *const predict_state = nullptr) -{ - // Total ops: 0 - - // Unused inputs - (void)dt; - - // Input arrays - - // Intermediate terms (0) - - // Output terms (1) - if (predict_state != nullptr) { - matrix::Matrix &_predict_state = (*predict_state); - - _predict_state(0, 0) = state(0, 0); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/vision_target_estimator/Orientation/python_derivation/generated/state.h b/src/modules/vision_target_estimator/Orientation/python_derivation/generated/state.h deleted file mode 100644 index 99e7da14d66d..000000000000 --- a/src/modules/vision_target_estimator/Orientation/python_derivation/generated/state.h +++ /dev/null @@ -1,36 +0,0 @@ -// -------------------------------------------------- -// This file was autogenerated, do NOT modify by hand -// -------------------------------------------------- - -#ifndef EKF_STATE_H -#define EKF_STATE_H - -#include - -namespace vtest -{ -struct StateSample { - float yaw{}; - - matrix::Vector Data() const - { - matrix::Vector state; - state.slice<1, 1>(0, 0) = yaw; - return state; - }; - - const matrix::Vector &vector() const - { - return *reinterpret_cast*>(const_cast(reinterpret_cast(&yaw))); - }; - -}; -static_assert(sizeof(matrix::Vector) == sizeof(StateSample), "state vector doesn't match StateSample size"); - -namespace State -{ -static constexpr uint8_t yaw{0}; -static constexpr uint8_t size{1}; -}; -} -#endif // !EKF_STATE_H diff --git a/src/modules/vision_target_estimator/Orientation/python_derivation/generated/syncState.h b/src/modules/vision_target_estimator/Orientation/python_derivation/generated/syncState.h deleted file mode 100644 index 74fd1eb49fbf..000000000000 --- a/src/modules/vision_target_estimator/Orientation/python_derivation/generated/syncState.h +++ /dev/null @@ -1,48 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym -{ - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: syncState - * - * Args: - * dt: Scalar - * state: Matrix11 - * - * Outputs: - * sync_state: Matrix11 - */ -template -void Syncstate(const Scalar dt, const matrix::Matrix &state, - matrix::Matrix *const sync_state = nullptr) -{ - // Total ops: 0 - - // Unused inputs - (void)dt; - - // Input arrays - - // Intermediate terms (0) - - // Output terms (1) - if (sync_state != nullptr) { - matrix::Matrix &_sync_state = (*sync_state); - - _sync_state(0, 0) = state(0, 0); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/vision_target_estimator/Orientation/python_derivation/utils/__init__.py b/src/modules/vision_target_estimator/Orientation/python_derivation/utils/__init__.py deleted file mode 100644 index e69de29bb2d1..000000000000 diff --git a/src/modules/vision_target_estimator/Orientation/python_derivation/utils/derivation_utils.py b/src/modules/vision_target_estimator/Orientation/python_derivation/utils/derivation_utils.py deleted file mode 100644 index 9788cc9a6c16..000000000000 --- a/src/modules/vision_target_estimator/Orientation/python_derivation/utils/derivation_utils.py +++ /dev/null @@ -1,163 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -""" - Copyright (c) 2022-2023 PX4 Development Team - 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. - -File: derivation_utils.py -Description: - Common functions used for the derivation of most estimators -""" - -import symforce.symbolic as sf - -import re - -def sign_no_zero(x) -> sf.Scalar: - """ - Returns -1 if x is negative, 1 if x is positive, and 1 if x is zero - """ - return 2 * sf.Min(sf.sign(x), 0) + 1 - -def add_epsilon_sign(expr, var, eps): - # Avoids a singularity at 0 while keeping the derivative correct - return expr.subs(var, var + eps * sign_no_zero(var)) - -def generate_px4_function(function_name, output_names): - from symforce.codegen import Codegen, CppConfig - import os - import fileinput - - codegen = Codegen.function( - function_name, - output_names=output_names, - config=CppConfig(zero_initialization_sparsity_threshold=1)) - metadata = codegen.generate_function( - output_dir="generated", - skip_directory_nesting=True) - - for f in metadata.generated_files: - print(" |- {}".format(os.path.relpath(f, metadata.output_dir))) - - # Replace cstdlib and Eigen functions by PX4 equivalents - with fileinput.FileInput(os.path.abspath(metadata.generated_files[0]), inplace=True) as file: - for line in file: - line = line.replace("std::max", "math::max") - line = line.replace("std::min", "math::min") - line = line.replace("Eigen", "matrix") - line = line.replace("matrix/Dense", "matrix/math.hpp") - - # don't allow underscore + uppercase identifier naming (always reserved for any use) - line = re.sub(r'_([A-Z])', lambda x: '_' + x.group(1).lower(), line) - - print(line, end='') - -def generate_python_function(function_name, output_names): - from symforce.codegen import Codegen, PythonConfig - codegen = Codegen.function( - function_name, - output_names=output_names, - config=PythonConfig()) - - metadata = codegen.generate_function( - output_dir="generated", - skip_directory_nesting=True) - -def build_state_struct(state, T="float"): - out = "struct StateSample {\n" - - def TypeFromLength(len): - if len == 1: - return f"{T}" - elif len == 2: - return f"matrix::Vector2<{T}>" - elif len == 3: - return f"matrix::Vector3<{T}>" - elif len == 4: - return f"matrix::Quaternion<{T}>" - else: - raise NotImplementedError - - for key, val in state.items(): - out += f"\t{TypeFromLength(val.storage_dim())} {key}{{}};\n" - - state_size = state.storage_dim() - out += f"\n\tmatrix::Vector<{T}, {state_size}> Data() const {{\n" \ - + f"\t\tmatrix::Vector<{T}, {state_size}> state;\n" - - index = state.index() - for key in index: - out += f"\t\tstate.slice<{index[key].storage_dim}, 1>({index[key].offset}, 0) = {key};\n" - - out += "\t\treturn state;\n" - out += "\t};\n" # Data - - # const ref vector access - first_field = next(iter(state)) - - out += f"\n\tconst matrix::Vector<{T}, {state_size}>& vector() const {{\n" \ - + f"\t\treturn *reinterpret_cast*>(const_cast(reinterpret_cast(&{first_field})));\n" \ - + f"\t}};\n\n" - - out += "};\n" # StateSample - - out += f"static_assert(sizeof(matrix::Vector<{T}, {state_size}>) == sizeof(StateSample), \"state vector doesn't match StateSample size\");\n" - - return out - -def build_tangent_state_struct(state, tangent_state_index): - out = "namespace State {\n" - - for key in tangent_state_index.keys(): - out += f"\tstatic constexpr uint8_t {key}{{{tangent_state_index[key].idx}}};\n" - - out += f"\tstatic constexpr uint8_t size{{{state.tangent_dim()}}};\n" - out += "};\n" # namespace State - return out - -def generate_px4_state(state, tangent_state_index): - print("Generate EKF tangent state definition") - filename = "state.h" - f = open(f"./generated/{filename}", "w") - header = ["// --------------------------------------------------\n", - "// This file was autogenerated, do NOT modify by hand\n", - "// --------------------------------------------------\n", - "\n#ifndef EKF_STATE_H", - "\n#define EKF_STATE_H\n\n", - "#include \n\n", - "namespace vtest\n{\n"] - f.writelines(header) - - f.write(build_state_struct(state)) - f.write("\n") - f.write(build_tangent_state_struct(state, tangent_state_index)) - - f.write("}\n") # namespace vtest - f.write("#endif // !EKF_STATE_H\n") - f.close() - print(f" |- {filename}") diff --git a/src/modules/vision_target_estimator/Position/python_derivation/KF_position.py b/src/modules/vision_target_estimator/Position/python_derivation/KF_position.py index 330b60e2fcae..6c62abcde974 100755 --- a/src/modules/vision_target_estimator/Position/python_derivation/KF_position.py +++ b/src/modules/vision_target_estimator/Position/python_derivation/KF_position.py @@ -126,7 +126,7 @@ def get_Phi_and_G(dt: sf.Scalar, state_pred[key] for key in state.keys_recursive() ]) - # Compute Jacobian A = ∂f/∂x + # Compute Phi = ∂f/∂x Phi = state_pred_matrix.jacobian([state]) # Compute G = ∂f/∂w G = state_pred_matrix.jacobian(acc)