EKF2: move yaw estimator to symforce

src/modules/ekf2/EKF/EKFGSF_yaw.cpp

@@ -34,6 +34,9 @@
 #include "EKFGSF_yaw.h"
 #include <cstdlib>
 
+#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<float, 3, 2> 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<float, 3> 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);

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"])

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 <matrix/math.hpp>
+
+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 <typename Scalar>
+void YawEstComputeMeasurementUpdate(const matrix::Matrix<Scalar, 3, 3>& P, const Scalar vel_obs_var,
+                                    const Scalar epsilon,
+                                    matrix::Matrix<Scalar, 2, 2>* const S_inv = nullptr,
+                                    Scalar* const S_det_inv = nullptr,
+                                    matrix::Matrix<Scalar, 3, 2>* const K = nullptr,
+                                    matrix::Matrix<Scalar, 3, 3>* 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<Scalar>(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<Scalar, 2, 2>& _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<Scalar, 3, 2>& _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<Scalar, 3, 3>& _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

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 <matrix/math.hpp>
+
+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 <typename Scalar>
+void YawEstPredictCovariance(const matrix::Matrix<Scalar, 3, 1>& state,
+                             const matrix::Matrix<Scalar, 3, 3>& P,
+                             const matrix::Matrix<Scalar, 2, 1>& d_vel, const Scalar d_vel_var,
+                             const Scalar d_ang_var,
+                             matrix::Matrix<Scalar, 3, 3>* 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<Scalar, 3, 3>& _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