ekf2-terrain: terrain is not a separate estimator

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)

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

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 <p_riseborough@live.com.au>
- * @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
- *
  */
 
 #include "ekf.h"

src/modules/ekf2/EKF/terrain_control.cpp

@@ -32,9 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file terrain_estimator.cpp
+ * @file terrain_control.cpp
- * Function for fusing rangefinder and optical flow measurements
- * to estimate terrain vertical position
  */
 
 #include "ekf.h"

src/modules/ekf2/EKF/terrain_estimator/derivation/derivation_terrain_estimator.py

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

src/modules/ekf2/EKF/terrain_estimator/derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h

@@ -1,66 +0,0 @@
-// -----------------------------------------------------------------------------
-// This file was autogenerated by symforce from template:
-//     function/FUNCTION.h.jinja
-// Do NOT modify by hand.
-// -----------------------------------------------------------------------------
-
-#pragma once
-
-#include <matrix/math.hpp>
-
-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 <typename Scalar>
-void TerrEstComputeFlowXyInnovVarAndHx(const Scalar terrain_vpos, const Scalar P,
-                                       const matrix::Matrix<Scalar, 4, 1>& q_att,
-                                       const matrix::Matrix<Scalar, 3, 1>& v, const Scalar pos_z,
-                                       const Scalar R, const Scalar epsilon,
-                                       matrix::Matrix<Scalar, 2, 1>* 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<Scalar>(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<Scalar, 2, 1>& _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

src/modules/ekf2/EKF/terrain_estimator/derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h

@@ -1,65 +0,0 @@
-// -----------------------------------------------------------------------------
-// This file was autogenerated by symforce from template:
-//     function/FUNCTION.h.jinja
-// Do NOT modify by hand.
-// -----------------------------------------------------------------------------
-
-#pragma once
-
-#include <matrix/math.hpp>
-
-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 <typename Scalar>
-void TerrEstComputeFlowYInnovVarAndH(const Scalar terrain_vpos, const Scalar P,
-                                     const matrix::Matrix<Scalar, 4, 1>& q_att,
-                                     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: 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<Scalar>(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

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)

src/modules/ekf2/test/test_EKF_terrain.cpp

@@ -32,8 +32,7 @@
  ****************************************************************************/
 
 /**
- * Test the terrain estimator
+ * Test the terrain estimate
- * @author Mathieu Bresciani <brescianimathieu@gmail.com>
  */
 
 #include <gtest/gtest.h>