diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py new file mode 100644 index 0000000000..29f2504133 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" + Copyright (c) 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_terrain_estimator.py +Description: +""" + +import symforce.symbolic as sf +from derivation_utils import * + +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 = quat_to_rot(q_att) + flow_pred = sf.V2() + dist = - (pos_z - terrain_vpos) + dist = 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...") +generate_px4_function(terr_est_compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"]) +generate_px4_function(terr_est_compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h new file mode 100644 index 0000000000..505802a863 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h @@ -0,0 +1,66 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// backends/cpp/templates/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: 28 + + // Input arrays + + // Intermediate terms (4) + const Scalar _tmp0 = std::pow(q_att(0, 0), Scalar(2)) - std::pow(q_att(1, 0), Scalar(2)) - + std::pow(q_att(2, 0), Scalar(2)) + std::pow(q_att(3, 0), Scalar(2)); + 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/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h new file mode 100644 index 0000000000..57058233e4 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h @@ -0,0 +1,65 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// backends/cpp/templates/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: 26 + + // Input arrays + + // Intermediate terms (3) + const Scalar _tmp0 = std::pow(q_att(0, 0), Scalar(2)) - std::pow(q_att(1, 0), Scalar(2)) - + std::pow(q_att(2, 0), Scalar(2)) + std::pow(q_att(3, 0), Scalar(2)); + 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/EKF/python/terrain_flow_derivation/derive_terrain_flow.py b/src/modules/ekf2/EKF/python/terrain_flow_derivation/derive_terrain_flow.py deleted file mode 100644 index 2ddf00621e..0000000000 --- a/src/modules/ekf2/EKF/python/terrain_flow_derivation/derive_terrain_flow.py +++ /dev/null @@ -1,97 +0,0 @@ -""" - -This script calculates the observation scalars (H matrix) for fusing optical flow -measurements for terrain estimation. - -@author: roman -""" - -from sympy import * - -# q: quaternion describing rotation from frame 1 to frame 2 -# returns a rotation matrix derived form q which describes the same -# rotation -def quat2Rot(q): - q0 = q[0] - q1 = q[1] - q2 = q[2] - q3 = q[3] - - Rot = Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)], - [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)], - [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]]) - - return Rot - -# take an expression calculated by the cse() method and write the expression -# into a text file in C format -def write_simplified(P_touple, filename, out_name): - subs = P_touple[0] - P = Matrix(P_touple[1]) - fd = open(filename, 'a') - - is_vector = P.shape[0] == 1 or P.shape[1] == 1 - - # write sub expressions - for index, item in enumerate(subs): - fd.write('float ' + str(item[0]) + ' = ' + str(item[1]) + ';\n') - - # write actual matrix values - fd.write('\n') - - if not is_vector: - iterator = range(0,sqrt(len(P)), 1) - for row in iterator: - for column in iterator: - fd.write(out_name + '(' + str(row) + ',' + str(column) + ') = ' + str(P[row, column]) + ';\n') - else: - iterator = range(0, len(P), 1) - - for item in iterator: - fd.write(out_name + '(' + str(item) + ') = ' + str(P[item]) + ';\n') - - fd.write('\n\n') - fd.close() - -########## Symbolic variable definition ####################################### - - -# vehicle velocity -v_x = Symbol("v_x", real=True) # vehicle body x velocity -v_y = Symbol("v_y", real=True) # vehicle body y velocity - -# unit quaternion describing vehicle attitude, qw is real part -qw = Symbol("q0", real=True) -qx = Symbol("q1", real=True) -qy = Symbol("q2", real=True) -qz = Symbol("q3", real=True) -q_att = Matrix([qw, qx, qy, qz]) - -# terrain vertial position in local NED frame -_terrain_vpos = Symbol("_terrain_vpos", real=True) - -_terrain_var = Symbol("_terrain_var", real=True) - -# vehicle vertical position in local NED frame -pos_z = Symbol("z", real=True) - -R_body_to_earth = quat2Rot(q_att) - -# Optical flow around x axis -flow_x = -v_y / (_terrain_vpos - pos_z) * R_body_to_earth[2,2] - -# Calculate observation scalar -H_x = Matrix([flow_x]).jacobian(Matrix([_terrain_vpos])) - -H_x_simple = cse(H_x, symbols('t0:30')) - -# Optical flow around y axis -flow_y = v_x / (_terrain_vpos - pos_z) * R_body_to_earth[2,2] - -# Calculate observation scalar -H_y = Matrix([flow_y]).jacobian(Matrix([_terrain_vpos])) - -H_y_simple = cse(H_y, symbols('t0:30')) - -write_simplified(H_x_simple, "flow_x_observation.txt", "Hx") -write_simplified(H_y_simple, "flow_y_observation.txt", "Hy") diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 127fe0f3fb..7d720e6beb 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 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. * @@ -33,13 +33,13 @@ /** * @file terrain_estimator.cpp - * Function for fusing rangefinder measurements to estimate terrain vertical position/ - * - * @author Paul Riseborough - * + * Function for fusing rangefinder and optical flow measurements + * to estimate terrain vertical position */ #include "ekf.h" +#include "python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h" +#include "python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h" #include @@ -254,6 +254,8 @@ void Ekf::controlHaglFlowFusion() } if (_flow_data_ready) { + updateOptFlow(_aid_src_optical_flow); + const bool continuing_conditions_passing = _control_status.flags.in_air && !_control_status.flags.opt_flow && _control_status.flags.gps @@ -320,90 +322,71 @@ void Ekf::fuseFlowForTerrain() { _aid_src_optical_flow.fusion_enabled = true; - // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed - // correct for gyro bias errors in the data used to do the motion compensation - // Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis. - const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt; + const float R_LOS = _aid_src_optical_flow.observation_variance[0]; - // get latest estimated orientation - const float q0 = _state.quat_nominal(0); - const float q1 = _state.quat_nominal(1); - const float q2 = _state.quat_nominal(2); - const float q3 = _state.quat_nominal(3); + // calculate the height above the ground of the optical flow camera. Since earth frame is NED + // a positive offset in earth frame leads to a smaller height above the ground. + float range = predictFlowRange(); - // calculate the optical flow observation variance - const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed); + const float state = _terrain_vpos; // linearize both axes using the same state value + Vector2f innov_var; + float H; + sym::TerrEstComputeFlowXyInnovVarAndHx(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &innov_var, &H); + innov_var.copyTo(_aid_src_optical_flow.innovation_variance); - // get rotation matrix from earth to body - const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal); - - // calculate the sensor position relative to the IMU - const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; - - // calculate the velocity of the sensor relative to the imu in body frame - // Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign - const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body; - - // calculate the velocity of the sensor in the earth frame - const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; - - // rotate into body frame - const Vector3f vel_body = earth_to_body * vel_rel_earth; - - // constrain terrain to minimum allowed value and predict height above ground - _terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2)); - const float pred_hagl_inv = 1.f / (_terrain_vpos - _state.pos(2)); - - // calculate prediced optical flow - const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) * pred_hagl_inv; - const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) * pred_hagl_inv; - - // calculate flow innovation - _aid_src_optical_flow.innovation[0] = pred_flow_x - opt_flow_rate(0); - _aid_src_optical_flow.innovation[1] = pred_flow_y - opt_flow_rate(1); - - const float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; - - // Calculate observation matrix for flow around - const float Hx = vel_body(1) * t0 * pred_hagl_inv * pred_hagl_inv; - const float Hy = -vel_body(0) * t0 * pred_hagl_inv * pred_hagl_inv; - - // Constrain terrain variance to be non-negative - _terrain_var = fmaxf(_terrain_var, sq(0.01f)); - - // Cacluate innovation variance - _aid_src_optical_flow.innovation_variance[0] = Hx * Hx * _terrain_var + R_LOS; - _aid_src_optical_flow.innovation_variance[1] = Hy * Hy * _terrain_var + R_LOS; + if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS) + || (_aid_src_optical_flow.innovation_variance[1] < R_LOS)) { + // we need to reinitialise the covariance matrix and abort this fusion step + ECL_ERR("Opt flow error - covariance reset"); + _terrain_var = 100.0f; + return; + } // run the innovation consistency check and record result setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f)); - // do not perform measurement update if badly conditioned + _innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f); + _innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f); + + // if either axis fails we abort the fusion if (_aid_src_optical_flow.innovation_rejected) { return; } - // calculate the kalman gain for the flow measurement - const float Kx = _terrain_var * Hx / _aid_src_optical_flow.innovation_variance[0]; + // fuse observation axes sequentially + for (uint8_t index = 0; index <= 1; index++) { + if (index == 0) { + // everything was already computed above - // calculate correction term for terrain variance - const float KxHxP = Kx * Hx * _terrain_var; + } else if (index == 1) { + // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) + sym::TerrEstComputeFlowYInnovVarAndH(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H); - _terrain_vpos += Kx * _aid_src_optical_flow.innovation[0]; - // guard against negative variance - _terrain_var = fmaxf(_terrain_var - KxHxP, sq(0.01f)); + // recalculate the innovation using the updated state + const Vector2f vel_body = predictFlowVelBody(); + range = predictFlowRange(); + _aid_src_optical_flow.innovation[1] = (-vel_body(0) / range) - _aid_src_optical_flow.observation[1]; + if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) { + // we need to reinitialise the covariance matrix and abort this fusion step + ECL_ERR("Opt flow error - covariance reset"); + _terrain_var = 100.0f; + return; + } + } - // calculate the kalman gain for the flow measurement - const float Ky = _terrain_var * Hy / _aid_src_optical_flow.innovation_variance[1]; + float Kfusion = _terrain_var * H / _aid_src_optical_flow.innovation_variance[index]; - // calculate correction term for terrain variance - const float KyHyP = Ky * Hy * _terrain_var; + _terrain_vpos += Kfusion * _aid_src_optical_flow.innovation[0]; + // constrain terrain to minimum allowed value and predict height above ground + _terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2)); - _terrain_vpos += Ky * _aid_src_optical_flow.innovation_variance[1]; - // guard against negative variance - _terrain_var = fmaxf(_terrain_var - KyHyP, sq(0.01f)); + // guard against negative variance + _terrain_var = fmaxf(_terrain_var - Kfusion * H * _terrain_var, sq(0.01f)); + } + _fault_status.flags.bad_optflow_X = false; + _fault_status.flags.bad_optflow_Y = false; _time_last_flow_terrain_fuse = _time_delayed_us; //_aid_src_optical_flow.time_last_fuse = _time_delayed_us; // TODO: separate aid source status for OF terrain?