mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
ekf2-terrain: terrain is not a separate estimator
This commit is contained in:
bresch
Silvan Fuhrer
parent
9001c23926
commit
7903ddf5df
@ -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)
|
||||
|
||||
@ -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})
|
||||
|
||||
@ -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"
|
||||
|
||||
@ -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"
|
||||
@ -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"])
|
||||
@ -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
|
||||
@ -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
|
||||
@ -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)
|
||||
|
||||
@ -32,8 +32,7 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* Test the terrain estimator
|
||||
* @author Mathieu Bresciani <brescianimathieu@gmail.com>
|
||||
* Test the terrain estimate
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
Loading…
x
Reference in New Issue
Block a user