diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 6d0f72dcbd..3f5d9d142f 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -213,7 +213,7 @@ if(CONFIG_EKF2_SIDESLIP) endif() if(CONFIG_EKF2_TERRAIN) - list(APPEND EKF_SRCS EKF/terrain_estimator/terrain_estimator.cpp) + list(APPEND EKF_SRCS EKF/terrain_control.cpp) endif() add_subdirectory(EKF) diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index bd9770c901..1134d1942b 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -135,7 +135,7 @@ if(CONFIG_EKF2_SIDESLIP) endif() if(CONFIG_EKF2_TERRAIN) - list(APPEND EKF_SRCS terrain_estimator/terrain_estimator.cpp) + list(APPEND EKF_SRCS terrain_control.cpp) endif() include_directories(${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp index bfed5a7fa1..8cd8f559f2 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp @@ -33,12 +33,6 @@ /** * @file optflow_fusion.cpp - * Function for fusing gps and baro measurements/ - * equations generated using EKF/python/ekf_derivation/main.py - * - * @author Paul Riseborough - * @author Siddharth Bharat Purohit - * */ #include "ekf.h" diff --git a/src/modules/ekf2/EKF/terrain_estimator/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_control.cpp similarity index 96% rename from src/modules/ekf2/EKF/terrain_estimator/terrain_estimator.cpp rename to src/modules/ekf2/EKF/terrain_control.cpp index 2afafce2d0..4de72ddd49 100644 --- a/src/modules/ekf2/EKF/terrain_estimator/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_control.cpp @@ -32,9 +32,7 @@ ****************************************************************************/ /** - * @file terrain_estimator.cpp - * Function for fusing rangefinder and optical flow measurements - * to estimate terrain vertical position + * @file terrain_control.cpp */ #include "ekf.h" diff --git a/src/modules/ekf2/EKF/terrain_estimator/derivation/derivation_terrain_estimator.py b/src/modules/ekf2/EKF/terrain_estimator/derivation/derivation_terrain_estimator.py deleted file mode 100755 index 8ecba2c03e..0000000000 --- a/src/modules/ekf2/EKF/terrain_estimator/derivation/derivation_terrain_estimator.py +++ /dev/null @@ -1,99 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" - 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: - - 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_terrain_estimator.py -Description: -""" - -import symforce -symforce.set_epsilon_to_symbol() - -import symforce.symbolic as sf - -# 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 - -def predict_opt_flow( - terrain_vpos: sf.Scalar, - q_att: sf.V4, - v: sf.V3, - pos_z: sf.Scalar, - epsilon : sf.Scalar -): - 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 = derivation_utils.add_epsilon_sign(dist, dist, epsilon) - flow_pred[0] = -v[1] / dist * R_to_earth[2, 2] - flow_pred[1] = v[0] / dist * R_to_earth[2, 2] - return flow_pred - -def terr_est_compute_flow_xy_innov_var_and_hx( - terrain_vpos: sf.Scalar, - P: sf.Scalar, - q_att: sf.V4, - v: sf.V3, - pos_z: sf.Scalar, - R: sf.Scalar, - epsilon : sf.Scalar -): - flow_pred = predict_opt_flow(terrain_vpos, q_att, v, pos_z, epsilon) - Hx = sf.V1(flow_pred[0]).jacobian(terrain_vpos) - Hy = sf.V1(flow_pred[1]).jacobian(terrain_vpos) - - innov_var = sf.V2() - innov_var[0] = (Hx * P * Hx.T + R)[0,0] - innov_var[1] = (Hy * P * Hy.T + R)[0,0] - - return (innov_var, Hx[0, 0]) - -def terr_est_compute_flow_y_innov_var_and_h( - terrain_vpos: sf.Scalar, - P: sf.Scalar, - q_att: sf.V4, - v: sf.V3, - pos_z: sf.Scalar, - R: sf.Scalar, - epsilon : sf.Scalar -): - flow_pred = predict_opt_flow(terrain_vpos, q_att, v, pos_z, epsilon) - Hy = sf.V1(flow_pred[1]).jacobian(terrain_vpos) - - innov_var = (Hy * P * Hy.T + R)[0,0] - - return (innov_var, Hy[0, 0]) - -print("Derive terrain estimator equations...") -derivation_utils.generate_px4_function(terr_est_compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"]) -derivation_utils.generate_px4_function(terr_est_compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"]) diff --git a/src/modules/ekf2/EKF/terrain_estimator/derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h b/src/modules/ekf2/EKF/terrain_estimator/derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h deleted file mode 100644 index 6b82a6534f..0000000000 --- a/src/modules/ekf2/EKF/terrain_estimator/derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h +++ /dev/null @@ -1,66 +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: terr_est_compute_flow_xy_innov_var_and_hx - * - * Args: - * terrain_vpos: Scalar - * P: Scalar - * q_att: Matrix41 - * v: Matrix31 - * pos_z: Scalar - * R: Scalar - * epsilon: Scalar - * - * Outputs: - * innov_var: Matrix21 - * H: Scalar - */ -template -void TerrEstComputeFlowXyInnovVarAndHx(const Scalar terrain_vpos, const Scalar P, - const matrix::Matrix& q_att, - const matrix::Matrix& v, const Scalar pos_z, - const Scalar R, const Scalar epsilon, - matrix::Matrix* const innov_var = nullptr, - Scalar* const H = nullptr) { - // Total ops: 27 - - // Input arrays - - // Intermediate terms (4) - 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); - const Scalar _tmp3 = P * std::pow(_tmp0, Scalar(2)) / std::pow(_tmp2, Scalar(4)); - - // Output terms (2) - if (innov_var != nullptr) { - matrix::Matrix& _innov_var = (*innov_var); - - _innov_var(0, 0) = R + _tmp3 * std::pow(v(1, 0), Scalar(2)); - _innov_var(1, 0) = R + _tmp3 * std::pow(v(0, 0), Scalar(2)); - } - - if (H != nullptr) { - Scalar& _h = (*H); - - _h = _tmp0 * v(1, 0) / std::pow(_tmp2, Scalar(2)); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/terrain_estimator/derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h b/src/modules/ekf2/EKF/terrain_estimator/derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h deleted file mode 100644 index 461db323cf..0000000000 --- a/src/modules/ekf2/EKF/terrain_estimator/derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h +++ /dev/null @@ -1,65 +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: terr_est_compute_flow_y_innov_var_and_h - * - * Args: - * terrain_vpos: Scalar - * P: Scalar - * q_att: Matrix41 - * v: Matrix31 - * pos_z: Scalar - * R: Scalar - * epsilon: Scalar - * - * Outputs: - * innov_var: Scalar - * H: Scalar - */ -template -void TerrEstComputeFlowYInnovVarAndH(const Scalar terrain_vpos, const Scalar P, - const matrix::Matrix& q_att, - 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: 25 - - // Input arrays - - // Intermediate terms (3) - 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); - - // Output terms (2) - if (innov_var != nullptr) { - Scalar& _innov_var = (*innov_var); - - _innov_var = - P * std::pow(_tmp0, Scalar(2)) * std::pow(v(0, 0), Scalar(2)) / std::pow(_tmp2, Scalar(4)) + - R; - } - - if (H != nullptr) { - Scalar& _h = (*H); - - _h = -_tmp0 * v(0, 0) / std::pow(_tmp2, Scalar(2)); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index bb3627dedb..424a2a208a 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -54,7 +54,7 @@ px4_add_unit_gtest(SRC test_EKF_mag.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_mag_declination_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_measurementSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_ringbuffer.cpp LINKLIBS ecl_EKF ecl_sensor_sim) -px4_add_unit_gtest(SRC test_EKF_terrain_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) +px4_add_unit_gtest(SRC test_EKF_terrain.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_utils.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_withReplayData.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_yaw_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) diff --git a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp b/src/modules/ekf2/test/test_EKF_terrain.cpp similarity index 98% rename from src/modules/ekf2/test/test_EKF_terrain_estimator.cpp rename to src/modules/ekf2/test/test_EKF_terrain.cpp index e07d4e8d5d..964cd43822 100644 --- a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_terrain.cpp @@ -32,8 +32,7 @@ ****************************************************************************/ /** - * Test the terrain estimator - * @author Mathieu Bresciani + * Test the terrain estimate */ #include