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

ekf2: symforce derivation allow optionally disabling mag and wind states

This commit is contained in:
Daniel Agar 2023-10-10 17:31:11 -04:00 committed by GitHub
parent b5f3d089c4
commit 5352a64042
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
15 changed files with 162 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
src/modules/ekf2/CMakeLists.txt
View File

@ -33,38 +33,85 @@
add_subdirectory(Utility)
option(EKF2_SYMFORCE_GEN "ekf2 generate symforce output" OFF)
# Symforce code generation TODO:fixme
# execute_process(
# COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic
# RESULT_VARIABLE PYTHON_SYMFORCE_EXIT_CODE
# OUTPUT_QUIET
# )
# if(${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0)
execute_process(
COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic
RESULT_VARIABLE PYTHON_SYMFORCE_EXIT_CODE
OUTPUT_QUIET
)
# set(EKF_DERIVATION_DIR ${CMAKE_CURRENT_SOURCE_DIR}/EKF/python/ekf_derivation)
# for now only provide symforce target helper if derivation.py generation isn't default
if((NOT CONFIG_EKF2_MAGNETOMETER) OR (NOT CONFIG_EKF2_WIND))
set(EKF2_SYMFORCE_GEN ON)
endif()
# set(EKF_GENERATED_SRC_FILES
# ${EKF_DERIVATION_DIR}/generated/compute_airspeed_h_and_k.h
# ${EKF_DERIVATION_DIR}/generated/compute_airspeed_innov_and_innov_var.h
# ${EKF_DERIVATION_DIR}/generated/compute_sideslip_h_and_k.h
# ${EKF_DERIVATION_DIR}/generated/compute_sideslip_innov_and_innov_var.h
# ${EKF_DERIVATION_DIR}/generated/covariance_prediction.h
# )
set(EKF_DERIVATION_SRC_DIR ${CMAKE_CURRENT_SOURCE_DIR}/EKF/python/ekf_derivation)
# add_custom_command(
# OUTPUT ${EKF_GENERATED_SRC_FILES}
# COMMAND ${PYTHON_EXECUTABLE} derivation.py
# DEPENDS
# ${EKF_DERIVATION_DIR}/derivation.py
# ${EKF_DERIVATION_DIR}/derivation_utils.py
set(EKF_GENERATED_FILES ${EKF_DERIVATION_SRC_DIR}/generated/state.h)
set(EKF_GENERATED_DERIVATION_INCLUDE_PATH "${EKF_DERIVATION_SRC_DIR}/..")
# WORKING_DIRECTORY ${EKF_DERIVATION_DIR}
# COMMENT "Symforce code generation"
# USES_TERMINAL
# )
if(EKF2_SYMFORCE_GEN AND (${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0))
# add_custom_target(ekf2_symforce_generate DEPENDS ${EKF_GENERATED_SRC_FILES})
# endif()
# regenerate default in tree
add_custom_command(
OUTPUT
${EKF_DERIVATION_SRC_DIR}/generated/predict_covariance.h
${EKF_DERIVATION_SRC_DIR}/generated/state.h
COMMAND
${PYTHON_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py
DEPENDS
${EKF_DERIVATION_SRC_DIR}/derivation.py
${EKF_DERIVATION_SRC_DIR}/derivation_utils.py
WORKING_DIRECTORY ${EKF_DERIVATION_SRC_DIR}
COMMENT "Symforce code generation (default)"
USES_TERMINAL
)
# generate to build directory
set(EKF_DERIVATION_DST_DIR ${CMAKE_CURRENT_BINARY_DIR}/ekf_derivation)
file(MAKE_DIRECTORY ${EKF_DERIVATION_DST_DIR})
set(EKF_GENERATED_FILES ${EKF_DERIVATION_DST_DIR}/generated/state.h)
set(EKF_GENERATED_DERIVATION_INCLUDE_PATH ${CMAKE_CURRENT_BINARY_DIR})
set(SYMFORCE_ARGS)
if(NOT CONFIG_EKF2_MAGNETOMETER)
message(STATUS "ekf2: symforce disabling mag")
list(APPEND SYMFORCE_ARGS "--disable_mag")
endif()
if(NOT CONFIG_EKF2_WIND)
message(STATUS "ekf2: symforce disabling wind")
list(APPEND SYMFORCE_ARGS "--disable_wind")
endif()
add_custom_command(
OUTPUT
${EKF_DERIVATION_DST_DIR}/generated/predict_covariance.h
${EKF_DERIVATION_DST_DIR}/generated/state.h
COMMAND
${PYTHON_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py ${SYMFORCE_ARGS}
DEPENDS
${EKF_DERIVATION_SRC_DIR}/derivation.py
${EKF_DERIVATION_SRC_DIR}/derivation_utils.py
WORKING_DIRECTORY ${EKF_DERIVATION_DST_DIR}
COMMENT "Symforce code generation"
USES_TERMINAL
)
add_custom_target(ekf2_symforce_generate
DEPENDS
${EKF_DERIVATION_SRC_DIR}/generated/predict_covariance.h
${EKF_DERIVATION_DST_DIR}/generated/predict_covariance.h
)
endif()
set(EKF_SRCS)
list(APPEND EKF_SRCS
@ -165,6 +212,7 @@ px4_add_module(
#-O0
INCLUDES
EKF
${EKF_GENERATED_DERIVATION_INCLUDE_PATH}
PRIORITY
"SCHED_PRIORITY_MAX - 18" # max priority below high priority WQ threads
STACK_MAX
@ -177,6 +225,8 @@ px4_add_module(
EKF2Selector.cpp
EKF2Selector.hpp
${EKF_GENERATED_FILES}
DEPENDS
geo
hysteresis

1
src/modules/ekf2/EKF/CMakeLists.txt
View File

@ -125,5 +125,6 @@ add_library(ecl_EKF
)
add_dependencies(ecl_EKF prebuild_targets)
target_include_directories(ecl_EKF PUBLIC ${EKF_GENERATED_DERIVATION_INCLUDE_PATH})
target_link_libraries(ecl_EKF PRIVATE geo world_magnetic_model)
target_compile_options(ecl_EKF PRIVATE -fno-associative-math)

6
src/modules/ekf2/EKF/airspeed_fusion.cpp
View File

@ -44,9 +44,9 @@
#include "ekf.h"
#include "python/ekf_derivation/generated/compute_airspeed_h_and_k.h"
#include "python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h"
#include "python/ekf_derivation/generated/compute_wind_init_and_cov_from_airspeed.h"
#include <ekf_derivation/generated/compute_airspeed_h_and_k.h>
#include <ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h>
#include <ekf_derivation/generated/compute_wind_init_and_cov_from_airspeed.h>
#include <mathlib/mathlib.h>

27
src/modules/ekf2/EKF/covariance.cpp
View File

@ -42,8 +42,8 @@
*/
#include "ekf.h"
#include "python/ekf_derivation/generated/predict_covariance.h"
#include "python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h"
#include <ekf_derivation/generated/predict_covariance.h>
#include <ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h>
#include <math.h>
#include <mathlib/mathlib.h>
@ -86,7 +86,9 @@ void Ekf::initialiseCovariance()
P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, accel_bias_var);
_prev_accel_bias_var.setAll(accel_bias_var);
#if defined(CONFIG_EKF2_MAGNETOMETER)
resetMagCov();
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
// wind
@ -236,8 +238,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
}
}
if (_control_status.flags.mag) {
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (_control_status.flags.mag) {
// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
if (P.trace<State::mag_I.dof>(State::mag_I.idx) < 0.1f) {
@ -261,7 +263,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
nextP(i, i) += mag_B_process_noise;
}
}
#endif // CONFIG_EKF2_MAGNETOMETER
} else {
// keep previous covariance
for (unsigned i = 0; i < State::mag_I.dof; i++) {
@ -278,6 +280,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
}
}
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
if (_control_status.flags.wind) {
@ -462,13 +465,13 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
}
#if defined(CONFIG_EKF2_MAGNETOMETER)
// magnetic field states
if (!_control_status.flags.mag) {
P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, 0.0f);
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, 0.0f);
} else {
#if defined(CONFIG_EKF2_MAGNETOMETER)
const float mag_I_var_max = 1.f;
constrainStateVar(State::mag_I, 0.f, mag_I_var_max);
@ -479,23 +482,23 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
P.makeRowColSymmetric<State::mag_I.dof>(State::mag_I.idx);
P.makeRowColSymmetric<State::mag_B.dof>(State::mag_B.idx);
}
#endif // CONFIG_EKF2_MAGNETOMETER
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
// wind velocity states
if (!_control_status.flags.wind) {
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, 0.0f);
} else {
#if defined(CONFIG_EKF2_WIND)
const float wind_vel_var_max = 1e6f;
constrainStateVar(State::wind_vel, 0.f, wind_vel_var_max);
if (force_symmetry) {
P.makeRowColSymmetric<State::wind_vel.dof>(State::wind_vel.idx);
}
#endif // CONFIG_EKF2_WIND
}
#endif // CONFIG_EKF2_WIND
}
void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
@ -543,9 +546,9 @@ void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
resetStateCovariance<State::quat_nominal>(q_cov);
}
#if defined(CONFIG_EKF2_MAGNETOMETER)
void Ekf::resetMagCov()
{
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (_mag_decl_cov_reset) {
ECL_INFO("reset mag covariance");
_mag_decl_cov_reset = false;
@ -555,12 +558,8 @@ void Ekf::resetMagCov()
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.mag_noise));
saveMagCovData();
#else
P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, 0.f);
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, 0.f);
#endif
}
#endif // CONFIG_EKF2_MAGNETOMETER
void Ekf::resetGyroBiasZCov()
{

4
src/modules/ekf2/EKF/drag_fusion.cpp
View File

@ -37,8 +37,8 @@
*/
#include "ekf.h"
#include "python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h"
#include "python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h"
#include <ekf_derivation/generated/compute_drag_x_innov_var_and_k.h>
#include <ekf_derivation/generated/compute_drag_y_innov_var_and_k.h>
#include <mathlib/mathlib.h>

2
src/modules/ekf2/EKF/ekf.cpp
View File

@ -68,7 +68,9 @@ void Ekf::reset()
_state.mag_B.setZero();
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
_state.wind_vel.setZero();
#endif // CONFIG_EKF2_WIND
#if defined(CONFIG_EKF2_RANGE_FINDER)
_range_sensor.setPitchOffset(_params.rng_sens_pitch);

9
src/modules/ekf2/EKF/ekf.h
View File

@ -49,7 +49,8 @@
#include "bias_estimator.hpp"
#include "height_bias_estimator.hpp"
#include "position_bias_estimator.hpp"
#include "python/ekf_derivation/generated/state.h"
#include <ekf_derivation/generated/state.h>
#include <uORB/topics/estimator_aid_source1d.h>
#include <uORB/topics/estimator_aid_source2d.h>
@ -1001,6 +1002,7 @@ private:
}
}
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (!_control_status.flags.mag) {
for (unsigned i = 0; i < State::mag_I.dof; i++) {
K(State::mag_I.idx + i) = 0.f;
@ -1012,12 +1014,15 @@ private:
K(State::mag_B.idx + i) = 0.f;
}
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
if (!_control_status.flags.wind) {
for (unsigned i = 0; i < State::wind_vel.dof; i++) {
K(State::wind_vel.idx + i) = 0.f;
}
}
#endif // CONFIG_EKF2_WIND
}
bool measurementUpdate(VectorState &K, float innovation_variance, float innovation)
@ -1177,7 +1182,9 @@ private:
void resetQuatCov(const float yaw_noise = NAN);
void resetQuatCov(const Vector3f &euler_noise_ned);
#if defined(CONFIG_EKF2_MAGNETOMETER)
void resetMagCov();
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
// perform a reset of the wind states and related covariances

8
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -40,8 +40,8 @@
*/
#include "ekf.h"
#include "python/ekf_derivation/generated/quat_var_to_rot_var.h"
#include "python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h"
#include <ekf_derivation/generated/quat_var_to_rot_var.h>
#include <ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h>
#include <mathlib/mathlib.h>
#include <lib/world_magnetic_model/geo_mag_declination.h>
@ -244,7 +244,9 @@ void Ekf::constrainStates()
_state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit());
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
_state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f);
#endif // CONFIG_EKF2_WIND
}
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
@ -817,7 +819,9 @@ void Ekf::fuse(const VectorState &K, float innovation)
_state.mag_B -= K.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) * innovation;
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
_state.wind_vel -= K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) * innovation;
#endif // CONFIG_EKF2_WIND
}
void Ekf::uncorrelateQuatFromOtherStates()

2
src/modules/ekf2/EKF/gps_yaw_fusion.cpp
View File

@ -41,7 +41,7 @@
*/
#include "ekf.h"
#include "python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h"
#include <ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h>
#include <mathlib/mathlib.h>
#include <cstdlib>

2
src/modules/ekf2/EKF/gravity_fusion.cpp
View File

@ -40,7 +40,7 @@
*/
#include "ekf.h"
#include "python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h"
#include <ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h>
#include <mathlib/mathlib.h>

10
src/modules/ekf2/EKF/mag_fusion.cpp
View File

@ -42,10 +42,12 @@
*/
#include "ekf.h"
#include "python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h"
#include "python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h"
#include "python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h"
#include "python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h"
#include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>
#include <ekf_derivation/generated/compute_mag_y_innov_var_and_h.h>
#include <ekf_derivation/generated/compute_mag_z_innov_var_and_h.h>
#include <ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h>
#include <mathlib/mathlib.h>

4
src/modules/ekf2/EKF/optflow_fusion.cpp
View File

@ -45,8 +45,8 @@
#include <mathlib/mathlib.h>
#include <float.h>
#include "python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h"
#include "python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h"
#include <ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h>
#include <ekf_derivation/generated/compute_flow_y_innov_var_and_h.h>
void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
{

50
src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
View File

@ -33,6 +33,8 @@ File: derivation.py
Description:
"""
import argparse
import symforce
symforce.set_epsilon_to_symbol()
@ -42,6 +44,15 @@ from symforce import ops
from symforce.values import Values
from derivation_utils import *
# Initialize parser
parser = argparse.ArgumentParser()
parser.add_argument("--disable_mag", action='store_true', help="disable mag")
parser.add_argument("--disable_wind", action='store_true', help="disable wind")
# Read arguments from command line
args = parser.parse_args()
# The state vector is organized in an ordered dictionary
State = Values(
quat_nominal = sf.V4(),
@ -54,6 +65,13 @@ State = Values(
wind_vel = sf.V2()
)
if args.disable_mag:
del State["mag_I"]
del State["mag_B"]
if args.disable_wind:
del State["wind_vel"]
class IdxDof():
def __init__(self, idx, dof):
self.idx = idx
@ -595,25 +613,31 @@ def rot_var_ned_to_lower_triangular_quat_cov(
print("Derive EKF2 equations...")
generate_px4_function(predict_covariance, output_names=["P_new"])
generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])
generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"])
generate_px4_function(compute_mag_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"])
generate_px4_function(compute_mag_y_innov_var_and_h, output_names=["innov_var", "H"])
generate_px4_function(compute_mag_z_innov_var_and_h, output_names=["innov_var", "H"])
generate_px4_function(compute_yaw_321_innov_var_and_h, output_names=["innov_var", "H"])
generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["innov_var", "H"])
if not args.disable_mag:
generate_px4_function(compute_mag_declination_pred_innov_var_and_h, output_names=["pred", "innov_var", "H"])
generate_px4_function(compute_mag_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"])
generate_px4_function(compute_mag_y_innov_var_and_h, output_names=["innov_var", "H"])
generate_px4_function(compute_mag_z_innov_var_and_h, output_names=["innov_var", "H"])
if not args.disable_wind:
generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"])
generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"])
generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"])
generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"])
generate_px4_function(compute_yaw_312_innov_var_and_h, output_names=["innov_var", "H"])
generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=["innov_var", "H"])
generate_px4_function(compute_mag_declination_pred_innov_var_and_h, output_names=["pred", "innov_var", "H"])
generate_px4_function(compute_yaw_321_innov_var_and_h, output_names=["innov_var", "H"])
generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=["innov_var", "H"])
generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"])
generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"])
generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"])
generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"])
generate_px4_function(quat_var_to_rot_var, output_names=["rot_var"])
generate_px4_function(rot_var_ned_to_lower_triangular_quat_cov, output_names=["q_cov_lower_triangle"])

4
src/modules/ekf2/EKF/sideslip_fusion.cpp
View File

@ -42,8 +42,8 @@
*/
#include "ekf.h"
#include "python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h"
#include "python/ekf_derivation/generated/compute_sideslip_h_and_k.h"
#include <ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h>
#include <ekf_derivation/generated/compute_sideslip_h_and_k.h>
#include <mathlib/mathlib.h>

4
src/modules/ekf2/EKF/yaw_fusion.cpp
View File

@ -33,8 +33,8 @@
#include "ekf.h"
#include "python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h"
#include "python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h"
#include <ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h>
#include <ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h>
#include <mathlib/mathlib.h>