diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp index e39ed9180a..7e2564fc19 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/EKFGSF_yaw.cpp @@ -283,6 +283,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang const float sin_yaw = sinf(_ekf_gsf[model_index].X(2)); 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; + const float daz = Vector3f(_ahrs_ekf_gsf[model_index].R * delta_ang)(2); // delta velocity process noise double if we're not in air const float accel_noise = in_air ? _accel_noise : 2.f * _accel_noise; @@ -292,7 +293,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang const float d_ang_var = sq(_gyro_noise * delta_ang_dt); 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); + Vector2f(dvx, dvy), d_vel_var, daz, 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); 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 index b950b54d09..e45fed5b25 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py @@ -1,7 +1,7 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- """ - Copyright (c) 2022 PX4 Development Team + Copyright (c) 2022-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: @@ -37,51 +37,86 @@ import symforce symforce.set_epsilon_to_symbol() import symforce.symbolic as sf +from symforce.values import Values from derivation_utils import * -class State: - vx = 0 - vy = 1 - yaw = 2 - n_states = 3 +State = Values( + vel = sf.V2(), + R = sf.Rot2() # 2D rotation to handle angle wrap +) -class VState(sf.Matrix): - SHAPE = (State.n_states, 1) +class VTangent(sf.Matrix): + SHAPE = (State.tangent_dim(), 1) -class MState(sf.Matrix): - SHAPE = (State.n_states, State.n_states) +class MTangent(sf.Matrix): + SHAPE = (State.tangent_dim(), State.tangent_dim()) + +def rot2_small_angle(angle: sf.V1): + # Approximation for small "delta angles" to avoid trigonometric functions + return sf.Rot2(sf.Complex(1, angle[0])) def yaw_est_predict_covariance( - state: VState, - P: MState, + state: VTangent, + P: MTangent, d_vel: sf.V2, d_vel_var: sf.Scalar, - d_ang_var: sf.Scalar + d_ang: sf.Scalar, + d_ang_var: sf.Scalar, ): - d_ang = sf.Symbol("d_ang") # does not appear in the jacobians + state = State.from_tangent(state) + d_ang = sf.V1(d_ang) # cast to vector to gain group properties (e.g.: to_tangent) - # 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])]]) + state_error = Values( + vel = sf.V2.symbolic("delta_vel"), + yaw = sf.V1.symbolic("delta_yaw") + ) - # attitude update equation - yaw_new = state[State.yaw] + d_ang + # True state kinematics + state_t = Values( + vel = state["vel"] + state_error["vel"], + R = state["R"] * rot2_small_angle(state_error["yaw"]) + ) - # velocity update equations - v_new = sf.V2(state[State.vx], state[State.vy]) + Tbn * d_vel + noise = Values( + d_vel = sf.V2.symbolic("a_n"), + d_ang = sf.V1.symbolic("w_n"), + ) - # Define vector of process equations - state_new = VState.block_matrix([[v_new], [sf.V1(yaw_new)]]) + input_t = Values( + d_vel = d_vel - noise["d_vel"], + d_ang = d_ang - noise["d_ang"] + ) + + state_t_pred = Values( + vel = state_t["vel"] + state_t["R"] * input_t["d_vel"], + R = state_t["R"] * rot2_small_angle(input_t["d_ang"]) + ) + + # Nominal state kinematics + state_pred = Values( + vel = state["vel"] + state["R"] * d_vel, + R = state["R"] * rot2_small_angle(d_ang) + ) + + # Error state kinematics + delta_rot = (state_pred["R"].inverse() * state_t_pred["R"]) + state_error_pred = Values( + vel = state_t_pred["vel"] - state_pred["vel"], + yaw = sf.simplify(delta_rot.z.imag) # small angle appriximation; use simplify to cancel R.T*R + ) + + zero_state_error = {state_error[key]: state_error[key].zero() for key in state_error.keys()} + zero_noise = {noise[key]: noise[key].zero() for key in noise.keys()} # Calculate state transition matrix - F = state_new.jacobian(state) + F = VTangent(state_error_pred.to_storage()).jacobian(state_error).subs(zero_state_error).subs(zero_noise) # 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 control(disturbance) influence matrix from IMU noise to error-state noise + G = VTangent(state_error_pred.to_storage()).jacobian(noise).subs(zero_state_error).subs(zero_noise) # derive the state error matrix var_u = sf.Matrix.diag([d_vel_var, d_vel_var, d_ang_var]) @@ -93,15 +128,15 @@ def yaw_est_predict_covariance( # 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): + for index in range(State.tangent_dim()): + for j in range(State.tangent_dim()): if index > j: P_new[index,j] = 0 return P_new def yaw_est_compute_measurement_update( - P: MState, + P: MTangent, vel_obs_var: sf.Scalar, epsilon : sf.Scalar ): @@ -123,8 +158,8 @@ def yaw_est_compute_measurement_update( # 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): + for index in range(State.tangent_dim()): + for j in range(State.tangent_dim()): if index > j: P_new[index,j] = 0 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 index 02f48e62b7..5d2c4327a5 100644 --- 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 @@ -20,6 +20,7 @@ namespace sym { * P: Matrix33 * d_vel: Matrix21 * d_vel_var: Scalar + * d_ang: Scalar * d_ang_var: Scalar * * Outputs: @@ -29,13 +30,13 @@ 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, + const Scalar d_ang, const Scalar d_ang_var, matrix::Matrix* const P_new = nullptr) { - // Total ops: 33 + // Total ops: 39 // Input arrays - // Intermediate terms (7) + // Intermediate terms (8) 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); @@ -44,6 +45,7 @@ void YawEstPredictCovariance(const matrix::Matrix& state, 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; + const Scalar _tmp7 = std::pow(d_ang, Scalar(2)) + 1; // Output terms (1) if (P_new != nullptr) { @@ -55,9 +57,9 @@ void YawEstPredictCovariance(const matrix::Matrix& state, _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; + _p_new(0, 2) = _tmp3 * _tmp7; + _p_new(1, 2) = _tmp6 * _tmp7; + _p_new(2, 2) = P(2, 2) * std::pow(_tmp7, Scalar(2)) + d_ang_var; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index 539ea0798f..84a665ab0e 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -57,7 +57,6 @@ px4_add_unit_gtest(SRC test_EKF_terrain_estimator.cpp LINKLIBS ecl_EKF ecl_senso 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) -px4_add_unit_gtest(SRC test_EKF_yaw_estimator_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_yaw_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) px4_add_unit_gtest(SRC test_SensorRangeFinder.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_drag_fusion.cpp LINKLIBS ecl_EKF ecl_sensor_sim) diff --git a/src/modules/ekf2/test/test_EKF_yaw_estimator_generated.cpp b/src/modules/ekf2/test/test_EKF_yaw_estimator_generated.cpp deleted file mode 100644 index a152275ca3..0000000000 --- a/src/modules/ekf2/test/test_EKF_yaw_estimator_generated.cpp +++ /dev/null @@ -1,230 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2022 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 - * 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. - * - ****************************************************************************/ - -#include -#include "EKF/ekf.h" -#include "test_helper/comparison_helper.h" - -#include "../EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h" -#include "../EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h" - -using namespace matrix; -typedef SquareMatrix SquareMatrix3f; - -SquareMatrix3f createRandomCovarianceMatrix3f() -{ - // Create a symmetric square matrix - SquareMatrix3f P; - - for (int col = 0; col <= 2; col++) { - for (int row = 0; row <= col; row++) { - if (row == col) { - P(row, col) = randf(); - - } else { - P(col, row) = P(row, col) = 2.0f * (randf() - 0.5f); - } - } - } - - // Make it positive definite - P = P.transpose() * P; - - return P; -} - -void sympyYawEstUpdate(const SquareMatrix3f &P, float velObsVar, SquareMatrix &S_inverse, - float &S_det_inverse, Matrix &K, SquareMatrix3f &P_new) -{ - const float P00 = P(0, 0); - const float P01 = P(0, 1); - const float P02 = P(0, 2); - const float P11 = P(1, 1); - const float P12 = P(1, 2); - const float P22 = P(2, 2); - - // optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py - const float t0 = powf(P01, 2.f); - const float t1 = -t0; - const float t2 = P00 * P11 + P00 * velObsVar + P11 * velObsVar + t1 + powf(velObsVar, 2.f); - - if (fabsf(t2) < 1e-6f) { - return; - } - - 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 = 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; - - S_det_inverse = t3; - - S_inverse(0, 0) = t3 * t4; - S_inverse(0, 1) = t6; - S_inverse(1, 1) = t3 * t7; - S_inverse(1, 0) = S_inverse(0, 1); - - 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); - - P_new(0, 0) = P00 - t16 * t19 - t20 * t8; - P_new(0, 1) = P01 * (t18 * t22 - t20 * velObsVar + 1); - P_new(1, 1) = P11 - t16 * t30 + t22 * t26; - P_new(0, 2) = P02 + t19 * t24 + t20 * t25; - P_new(1, 2) = P12 + t23 * t27 + t30 * t31; - P_new(2, 2) = P22 - t23 * t33 - t25 * t36; - P_new(1, 0) = P_new(0, 1); - P_new(2, 0) = P_new(0, 2); - P_new(2, 1) = P_new(1, 2); -} - -void sympyYawEstPrediction(const Vector3f &state, const SquareMatrix3f &P, const Vector2f &d_vel, float d_vel_var, - float d_ang_var, SquareMatrix3f &P_new) -{ - const float P00 = P(0, 0); - const float P01 = P(0, 1); - const float P02 = P(0, 2); - const float P11 = P(1, 1); - const float P12 = P(1, 2); - const float P22 = P(2, 2); - const float psi = state(2); - const float dvx = d_vel(0); - const float dvy = d_vel(1); - const float dvxVar = d_vel_var; - const float dvyVar = d_vel_var; - const float dazVar = d_ang_var; - - const float S0 = cosf(psi); - const float S1 = powf(S0, 2.f); - const float S2 = sinf(psi); - const float S3 = powf(S2, 2.f); - 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; - - P_new(0, 0) = P00 - P02 * S4 + S1 * dvxVar + S3 * dvyVar - S4 * S5; - P_new(0, 1) = -P12 * S4 + S5 * S6 + S8; - P_new(1, 1) = P11 + P12 * S6 + S1 * dvyVar + S3 * dvxVar + S6 * S9; - P_new(0, 2) = S5; - P_new(1, 2) = S9; - P_new(2, 2) = P22 + dazVar; -} - -TEST(YawEstimatorGenerated, SympyVsSymforceUpdate) -{ - const float R = sq(0.1f); - - SquareMatrix P = createRandomCovarianceMatrix3f(); - - SquareMatrix innov_var_inv_sympy; - float innov_var_det_inv_sympy; - SquareMatrix3f P_new_sympy; - Matrix K_sympy; - sympyYawEstUpdate(P, R, innov_var_inv_sympy, innov_var_det_inv_sympy, K_sympy, P_new_sympy); - - SquareMatrix innov_var_inv_symforce; - float innov_var_det_inv_symforce; - SquareMatrix3f P_new_symforce; - Matrix K_symforce; - sym::YawEstComputeMeasurementUpdate(P, R, FLT_EPSILON, - &innov_var_inv_symforce, - &innov_var_det_inv_symforce, &K_symforce, &P_new_symforce); - // copy upper to lower diagonal - P_new_symforce(1, 0) = P_new_symforce(0, 1); - P_new_symforce(2, 0) = P_new_symforce(0, 2); - P_new_symforce(2, 1) = P_new_symforce(1, 2); - - EXPECT_FLOAT_EQ(innov_var_det_inv_sympy, innov_var_det_inv_symforce); - EXPECT_TRUE(isEqual(P_new_sympy, P_new_symforce)); - EXPECT_TRUE(isEqual(K_sympy, K_symforce)); - EXPECT_TRUE(isEqual(innov_var_inv_sympy, innov_var_inv_symforce)); -} - -TEST(YawEstimatorGenerated, SympyVsSymforcePrediction) -{ - const float dt = 0.01f; - const Vector2f d_vel = Vector2f(0.1f, -0.5f) * dt; - const float d_vel_var = sq(2.f * dt); - const float d_ang_var = sq(0.1f * dt); - - const Vector3f state(3.f, 10.f, M_PI_F / 2.f); - - SquareMatrix P = createRandomCovarianceMatrix3f(); - SquareMatrix P_new_symforce; - - sym::YawEstPredictCovariance(state, P, d_vel, d_vel_var, d_ang_var, &P_new_symforce); - - EXPECT_GT(P_new_symforce(0, 0), P(0, 0)); - EXPECT_GT(P_new_symforce(1, 1), P(1, 1)); - EXPECT_GT(P_new_symforce(2, 2), P(2, 2)); - - SquareMatrix P_new_sympy; - sympyYawEstPrediction(state, P, d_vel, d_vel_var, d_ang_var, P_new_sympy); - - EXPECT_TRUE(isEqual(P_new_sympy, P_new_symforce)); -}