From 7c3301951083a123460c0aee748797d2a89c30a7 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 29 Nov 2022 16:10:00 +0100 Subject: [PATCH] EKF2: move yaw estimator to symforce --- src/modules/ekf2/EKF/EKFGSF_yaw.cpp | 129 +++-------------- .../derivation_yaw_estimator.py | 132 ++++++++++++++++++ .../yaw_est_compute_measurement_update.h | 102 ++++++++++++++ .../generated/yaw_est_predict_covariance.h | 65 +++++++++ 4 files changed, 322 insertions(+), 106 deletions(-) create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp index 859796726a..8bd5c51de1 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp @@ -34,6 +34,9 @@ #include "EKFGSF_yaw.h" #include +#include "python/ekf_derivation/generated/yaw_est_predict_covariance.h" +#include "python/ekf_derivation/generated/yaw_est_compute_measurement_update.h" + EKFGSF_yaw::EKFGSF_yaw() { initialiseEKFGSF(); @@ -271,44 +274,11 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang const float dvx = del_vel_NED(0) * cos_yaw + del_vel_NED(1) * sin_yaw; const float dvy = - del_vel_NED(0) * sin_yaw + del_vel_NED(1) * cos_yaw; - // sum delta velocities in earth frame: - _ekf_gsf[model_index].X(0) += del_vel_NED(0); - _ekf_gsf[model_index].X(1) += del_vel_NED(1); - - // predict covariance - equations generated using EKF/python/gsf_ekf_yaw_estimator/main.py - - // Local short variable name copies required for readability - const float P00 = _ekf_gsf[model_index].P(0, 0); - const float P01 = _ekf_gsf[model_index].P(0, 1); - const float P02 = _ekf_gsf[model_index].P(0, 2); - const float P11 = _ekf_gsf[model_index].P(1, 1); - const float P12 = _ekf_gsf[model_index].P(1, 2); - const float P22 = _ekf_gsf[model_index].P(2, 2); - const float psi = _ekf_gsf[model_index].X(2); - // Use fixed values for delta velocity and delta angle process noise variances - const float dvxVar = sq(_accel_noise * delta_vel_dt); // variance of forward delta velocity - (m/s)^2 - const float dvyVar = dvxVar; // variance of right delta velocity - (m/s)^2 - const float dazVar = sq(_gyro_noise * delta_ang_dt); // variance of yaw delta angle - rad^2 + const float d_vel_var = sq(_accel_noise * delta_vel_dt); + const float d_ang_var = sq(_gyro_noise * delta_ang_dt); - // optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py - const float S0 = cosf(psi); - const float S1 = ecl::powf(S0, 2); - const float S2 = sinf(psi); - const float S3 = ecl::powf(S2, 2); - const float S4 = S0 * dvy + S2 * dvx; - const float S5 = P02 - P22 * S4; - const float S6 = S0 * dvx - S2 * dvy; - const float S7 = S0 * S2; - const float S8 = P01 + S7 * dvxVar - S7 * dvyVar; - const float S9 = P12 + P22 * S6; - - _ekf_gsf[model_index].P(0, 0) = P00 - P02 * S4 + S1 * dvxVar + S3 * dvyVar - S4 * S5; - _ekf_gsf[model_index].P(0, 1) = -P12 * S4 + S5 * S6 + S8; - _ekf_gsf[model_index].P(1, 1) = P11 + P12 * S6 + S1 * dvyVar + S3 * dvxVar + S6 * S9; - _ekf_gsf[model_index].P(0, 2) = S5; - _ekf_gsf[model_index].P(1, 2) = S9; - _ekf_gsf[model_index].P(2, 2) = P22 + dazVar; + sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P, Vector2f(dvx, dvy), d_vel_var, d_ang_var, &_ekf_gsf[model_index].P); // covariance matrix is symmetrical, so copy upper half to lower half _ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1); @@ -321,89 +291,36 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang for (unsigned index = 0; index < 3; index++) { _ekf_gsf[model_index].P(index, index) = fmaxf(_ekf_gsf[model_index].P(index, index), min_var); } + + // sum delta velocities in earth frame: + _ekf_gsf[model_index].X(0) += del_vel_NED(0); + _ekf_gsf[model_index].X(1) += del_vel_NED(1); } // Update EKF states and covariance for specified model index using velocity measurement bool EKFGSF_yaw::updateEKF(const uint8_t model_index, const Vector2f &vel_NE, const float vel_accuracy) { // set observation variance from accuracy estimate supplied by GPS and apply a sanity check minimum - const float velObsVar = sq(fmaxf(vel_accuracy, 0.01f)); + const float vel_obs_var = sq(fmaxf(vel_accuracy, 0.01f)); // calculate velocity observation innovations _ekf_gsf[model_index].innov(0) = _ekf_gsf[model_index].X(0) - vel_NE(0); _ekf_gsf[model_index].innov(1) = _ekf_gsf[model_index].X(1) - vel_NE(1); - // Use temporary variables for covariance elements to reduce verbosity of auto-code expressions - const float P00 = _ekf_gsf[model_index].P(0, 0); - const float P01 = _ekf_gsf[model_index].P(0, 1); - const float P02 = _ekf_gsf[model_index].P(0, 2); - const float P11 = _ekf_gsf[model_index].P(1, 1); - const float P12 = _ekf_gsf[model_index].P(1, 2); - const float P22 = _ekf_gsf[model_index].P(2, 2); - - // optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py - const float t0 = ecl::powf(P01, 2); - const float t1 = -t0; - const float t2 = P00 * P11 + P00 * velObsVar + P11 * velObsVar + t1 + ecl::powf(velObsVar, 2); - - if (fabsf(t2) < 1e-6f) { - return false; - } - - const float t3 = 1.0F / t2; - const float t4 = P11 + velObsVar; - const float t5 = P01 * t3; - const float t6 = -t5; - const float t7 = P00 + velObsVar; - const float t8 = P00 * t4 + t1; - const float t9 = t5 * velObsVar; - const float t10 = P11 * t7; - const float t11 = t1 + t10; - const float t12 = P01 * P12; - const float t13 = P02 * t4; - const float t14 = P01 * P02; - const float t15 = P12 * t7; - const float t16 = t0 * velObsVar; - const float t17 = ecl::powf(t2, -2); - const float t18 = t4 * velObsVar + t8; - const float t19 = t17 * t18; - const float t20 = t17 * (t16 + t7 * t8); - const float t21 = t0 - t10; - const float t22 = t17 * t21; - const float t23 = t14 - t15; - const float t24 = P01 * t23; - const float t25 = t12 - t13; - const float t26 = t16 - t21 * t4; - const float t27 = t17 * t26; - const float t28 = t11 + t7 * velObsVar; - const float t30 = t17 * t28; - const float t31 = P01 * t25; - const float t32 = t23 * t4 + t31; - const float t33 = t17 * t32; - const float t35 = t24 + t25 * t7; - const float t36 = t17 * t35; - - _ekf_gsf[model_index].S_det_inverse = t3; - - _ekf_gsf[model_index].S_inverse(0, 0) = t3 * t4; - _ekf_gsf[model_index].S_inverse(0, 1) = t6; - _ekf_gsf[model_index].S_inverse(1, 1) = t3 * t7; - _ekf_gsf[model_index].S_inverse(1, 0) = _ekf_gsf[model_index].S_inverse(0, 1); - matrix::Matrix K; - K(0, 0) = t3 * t8; - K(1, 0) = t9; - K(2, 0) = t3 * (-t12 + t13); - K(0, 1) = t9; - K(1, 1) = t11 * t3; - K(2, 1) = t3 * (-t14 + t15); + matrix::SquareMatrix P_new; - _ekf_gsf[model_index].P(0, 0) = P00 - t16 * t19 - t20 * t8; - _ekf_gsf[model_index].P(0, 1) = P01 * (t18 * t22 - t20 * velObsVar + 1); - _ekf_gsf[model_index].P(1, 1) = P11 - t16 * t30 + t22 * t26; - _ekf_gsf[model_index].P(0, 2) = P02 + t19 * t24 + t20 * t25; - _ekf_gsf[model_index].P(1, 2) = P12 + t23 * t27 + t30 * t31; - _ekf_gsf[model_index].P(2, 2) = P22 - t23 * t33 - t25 * t36; + sym::YawEstComputeMeasurementUpdate(_ekf_gsf[model_index].P, + vel_obs_var, + FLT_EPSILON, + &_ekf_gsf[model_index].S_inverse, + &_ekf_gsf[model_index].S_det_inverse, + &K, + &P_new); + + _ekf_gsf[model_index].P = P_new; + + // copy upper to lower diagonal _ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1); _ekf_gsf[model_index].P(2, 0) = _ekf_gsf[model_index].P(0, 2); _ekf_gsf[model_index].P(2, 1) = _ekf_gsf[model_index].P(1, 2); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py new file mode 100644 index 0000000000..1b0bdb9076 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" + Copyright (c) 2022 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_yaw_estimator.py +Description: +""" + +import symforce.symbolic as sf +from derivation_utils import * + +class State: + vx = 0 + vy = 1 + yaw = 2 + n_states = 3 + +class VState(sf.Matrix): + SHAPE = (State.n_states, 1) + +class MState(sf.Matrix): + SHAPE = (State.n_states, State.n_states) + +def yaw_est_predict_covariance( + state: VState, + P: MState, + d_vel: sf.V2, + d_vel_var: sf.Scalar, + d_ang_var: sf.Scalar +): + d_ang = sf.Symbol("d_ang") # does not appear in the jacobians + + # derive the body to nav direction transformation matrix + Tbn = sf.Matrix([[sf.cos(state[State.yaw]) , -sf.sin(state[State.yaw])], + [sf.sin(state[State.yaw]) , sf.cos(state[State.yaw])]]) + + # attitude update equation + yaw_new = state[State.yaw] + d_ang + + # velocity update equations + v_new = sf.V2(state[State.vx], state[State.vy]) + Tbn * d_vel + + # Define vector of process equations + state_new = VState.block_matrix([[v_new], [sf.V1(yaw_new)]]) + + # Calculate state transition matrix + F = state_new.jacobian(state) + + # Derive the covariance prediction equations + # Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and + # velocities, after bias effects have been removed. + + # derive the control(disturbance) influence matrix from IMU noise to state noise + G = state_new.jacobian(sf.V3.block_matrix([[d_vel], [sf.V1(d_ang)]])) + + # derive the state error matrix + var_u = sf.Matrix.diag([d_vel_var, d_vel_var, d_ang_var]) + + Q = G * var_u * G.T + + P_new = F * P * F.T + Q + + # Generate the equations for the upper triangular matrix and the diagonal only + # Since the matrix is symmetric, the lower triangle does not need to be derived + # and can simply be copied in the implementation + for index in range(State.n_states): + for j in range(State.n_states): + if index > j: + P_new[index,j] = 0 + + return P_new + +def yaw_est_compute_measurement_update( + P: MState, + vel_obs_var: sf.Scalar, + epsilon : sf.Scalar +): + H = sf.Matrix([[1,0,0], + [0,1,0]]) + + R = sf.Matrix([[vel_obs_var , 0], + [0 , vel_obs_var]]) + + S = H * P * H.T + R + S_det = S[0, 0] * S[1, 1] - S[1, 0] * S[0, 1] + S_det_inv = add_epsilon_sign(1 / S_det, S_det, epsilon) + + # Compute inverse using simple formula for 2x2 matrix and using protected division + S_inv = sf.M22([[S[1, 1], -S[0, 1]], [-S[1, 0], S[0, 0]]]) * S_det_inv + K = (P * H.T) * S_inv + P_new = P - K * H * P + + # Generate the equations for the upper triangular matrix and the diagonal only + # Since the matrix is symmetric, the lower triangle does not need to be derived + # and can simply be copied in the implementation + for index in range(State.n_states): + for j in range(State.n_states): + if index > j: + P_new[index,j] = 0 + + return (S_inv, S_det_inv, K, P_new) + +print("Derive yaw estimator equations...") +generate_px4_function(yaw_est_predict_covariance, output_names=["P_new"]) +generate_px4_function(yaw_est_compute_measurement_update, output_names=["S_inv", "S_det_inv", "K", "P_new"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h new file mode 100644 index 0000000000..abd6238760 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h @@ -0,0 +1,102 @@ +// ----------------------------------------------------------------------------- +// 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: yaw_est_compute_measurement_update + * + * Args: + * P: Matrix33 + * vel_obs_var: Scalar + * epsilon: Scalar + * + * Outputs: + * S_inv: Matrix22 + * S_det_inv: Scalar + * K: Matrix32 + * P_new: Matrix33 + */ +template +void YawEstComputeMeasurementUpdate(const matrix::Matrix& P, const Scalar vel_obs_var, + const Scalar epsilon, + matrix::Matrix* const S_inv = nullptr, + Scalar* const S_det_inv = nullptr, + matrix::Matrix* const K = nullptr, + matrix::Matrix* const P_new = nullptr) { + // Total ops: 60 + + // Input arrays + + // Intermediate terms (15) + const Scalar _tmp0 = P(1, 1) + vel_obs_var; + const Scalar _tmp1 = P(0, 0) + vel_obs_var; + const Scalar _tmp2 = -P(0, 1) * P(1, 0) + _tmp0 * _tmp1; + const Scalar _tmp3 = + Scalar(1.0) / + (_tmp2 + epsilon * (2 * math::min(0, (((_tmp2) > 0) - ((_tmp2) < 0))) + 1)); + const Scalar _tmp4 = _tmp0 * _tmp3; + const Scalar _tmp5 = P(1, 0) * _tmp3; + const Scalar _tmp6 = P(0, 1) * _tmp3; + const Scalar _tmp7 = _tmp1 * _tmp3; + const Scalar _tmp8 = -P(0, 1) * _tmp5; + const Scalar _tmp9 = P(0, 0) * _tmp4 + _tmp8; + const Scalar _tmp10 = -P(1, 1) * _tmp5 + _tmp0 * _tmp5; + const Scalar _tmp11 = P(2, 0) * _tmp4 - P(2, 1) * _tmp5; + const Scalar _tmp12 = -P(0, 0) * _tmp6 + _tmp1 * _tmp6; + const Scalar _tmp13 = P(1, 1) * _tmp7 + _tmp8; + const Scalar _tmp14 = -P(2, 0) * _tmp6 + P(2, 1) * _tmp7; + + // Output terms (4) + if (S_inv != nullptr) { + matrix::Matrix& _S_inv = (*S_inv); + + _S_inv(0, 0) = _tmp4; + _S_inv(1, 0) = -_tmp5; + _S_inv(0, 1) = -_tmp6; + _S_inv(1, 1) = _tmp7; + } + + if (S_det_inv != nullptr) { + Scalar& _S_det_inv = (*S_det_inv); + + _S_det_inv = _tmp3; + } + + if (K != nullptr) { + matrix::Matrix& _K = (*K); + + _K(0, 0) = _tmp9; + _K(1, 0) = _tmp10; + _K(2, 0) = _tmp11; + _K(0, 1) = _tmp12; + _K(1, 1) = _tmp13; + _K(2, 1) = _tmp14; + } + + if (P_new != nullptr) { + matrix::Matrix& _P_new = (*P_new); + + _P_new(0, 0) = -P(0, 0) * _tmp9 + P(0, 0) - P(1, 0) * _tmp12; + _P_new(1, 0) = 0; + _P_new(2, 0) = 0; + _P_new(0, 1) = -P(0, 1) * _tmp9 + P(0, 1) - P(1, 1) * _tmp12; + _P_new(1, 1) = -P(0, 1) * _tmp10 - P(1, 1) * _tmp13 + P(1, 1); + _P_new(2, 1) = 0; + _P_new(0, 2) = -P(0, 2) * _tmp9 + P(0, 2) - P(1, 2) * _tmp12; + _P_new(1, 2) = -P(0, 2) * _tmp10 - P(1, 2) * _tmp13 + P(1, 2); + _P_new(2, 2) = -P(0, 2) * _tmp11 - P(1, 2) * _tmp14 + P(2, 2); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h new file mode 100644 index 0000000000..b2c4a3665c --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.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: yaw_est_predict_covariance + * + * Args: + * state: Matrix31 + * P: Matrix33 + * d_vel: Matrix21 + * d_vel_var: Scalar + * d_ang_var: Scalar + * + * Outputs: + * P_new: Matrix33 + */ +template +void YawEstPredictCovariance(const matrix::Matrix& state, + const matrix::Matrix& P, + const matrix::Matrix& d_vel, const Scalar d_vel_var, + const Scalar d_ang_var, + matrix::Matrix* const P_new = nullptr) { + // Total ops: 33 + + // Input arrays + + // Intermediate terms (7) + const Scalar _tmp0 = std::cos(state(2, 0)); + const Scalar _tmp1 = std::sin(state(2, 0)); + const Scalar _tmp2 = -_tmp0 * d_vel(1, 0) - _tmp1 * d_vel(0, 0); + const Scalar _tmp3 = P(0, 2) + P(2, 2) * _tmp2; + const Scalar _tmp4 = + std::pow(_tmp0, Scalar(2)) * d_vel_var + std::pow(_tmp1, Scalar(2)) * d_vel_var; + const Scalar _tmp5 = _tmp0 * d_vel(0, 0) - _tmp1 * d_vel(1, 0); + const Scalar _tmp6 = P(1, 2) + P(2, 2) * _tmp5; + + // Output terms (1) + if (P_new != nullptr) { + matrix::Matrix& _P_new = (*P_new); + + _P_new(0, 0) = P(0, 0) + P(2, 0) * _tmp2 + _tmp2 * _tmp3 + _tmp4; + _P_new(1, 0) = 0; + _P_new(2, 0) = 0; + _P_new(0, 1) = P(0, 1) + P(2, 1) * _tmp2 + _tmp3 * _tmp5; + _P_new(1, 1) = P(1, 1) + P(2, 1) * _tmp5 + _tmp4 + _tmp5 * _tmp6; + _P_new(2, 1) = 0; + _P_new(0, 2) = _tmp3; + _P_new(1, 2) = _tmp6; + _P_new(2, 2) = P(2, 2) + d_ang_var; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym