Update ekf derivations to SymForce 0.9.0

Tools/setup/optional-requirements.txt

@@ -1 +1 @@
-symforce>=0.5.0
+symforce>=0.9.0

src/lib/wind_estimator/python/derivation.py

@@ -34,42 +34,43 @@
     Derivation of a wind and airspeed scale (EKF) estimator using SymForce
 """
 
-from symforce import symbolic as sm
-from symforce import geo
-from symforce import typing as T
+import symforce
+symforce.set_epsilon_to_symbol()
+
+import symforce.symbolic as sf
 from derivation_utils import *
 
 def fuse_airspeed(
-        v_local: geo.V3,
-        state: geo.V3,
-        P: geo.M33,
-        airspeed: T.Scalar,
-        R: T.Scalar,
-        epsilon: T.Scalar
-) -> (geo.V3, geo.V3, T.Scalar, T.Scalar):
-
-    vel_rel = geo.V3(v_local[0] - state[0], v_local[1] - state[1], v_local[2])
+        v_local: sf.V3,
+        state: sf.V3,
+        P: sf.M33,
+        airspeed: sf.Scalar,
+        R: sf.Scalar,
+        epsilon: sf.Scalar
+) -> (sf.M13, sf.M31, sf.Scalar, sf.Scalar):
+
+    vel_rel = sf.V3(v_local[0] - state[0], v_local[1] - state[1], v_local[2])
     airspeed_pred = vel_rel.norm(epsilon=epsilon) * state[2]
 
     innov = airspeed - airspeed_pred
 
-    H = geo.V1(airspeed_pred).jacobian(state)
+    H = sf.V1(airspeed_pred).jacobian(state)
     innov_var = (H * P * H.T + R)[0,0]
 
     K = P * H.T / sm.Max(innov_var, epsilon)
 
-    return (geo.V3(H), K, innov_var, innov)
+    return (H, K, innov_var, innov)
 
 def fuse_beta(
-        v_local: geo.V3,
-        state: geo.V3,
-        P: geo.M33,
-        q_att: geo.V4,
-        R: T.Scalar,
-        epsilon: T.Scalar
-) -> (geo.V3, geo.V3, T.Scalar, T.Scalar):
-
-    vel_rel = geo.V3(v_local[0] - state[0], v_local[1] - state[1], v_local[2])
+        v_local: sf.V3,
+        state: sf.V3,
+        P: sf.M33,
+        q_att: sf.V4,
+        R: sf.Scalar,
+        epsilon: sf.Scalar
+) -> (sf.M13, sf.M31, sf.Scalar, sf.Scalar):
+
+    vel_rel = sf.V3(v_local[0] - state[0], v_local[1] - state[1], v_local[2])
     relative_wind_body = quat_to_rot(q_att).T * vel_rel
 
     # Small angle approximation of side slip model
@@ -78,29 +79,29 @@ def fuse_beta(
 
     innov = 0.0 - beta_pred
 
-    H = geo.V1(beta_pred).jacobian(state)
+    H = sf.V1(beta_pred).jacobian(state)
     innov_var = (H * P * H.T + R)[0,0]
 
     K = P * H.T / sm.Max(innov_var, epsilon)
 
-    return (geo.V3(H), K, innov_var, innov)
+    return (H, K, innov_var, innov)
 
 def init_wind_using_airspeed(
-        v_local: geo.V3,
-        heading: T.Scalar,
-        airspeed: T.Scalar,
-        v_var: T.Scalar,
-        heading_var: T.Scalar,
-        sideslip_var: T.Scalar,
-        airspeed_var: T.Scalar,
-) -> (geo.V2, geo.M22):
+        v_local: sf.V3,
+        heading: sf.Scalar,
+        airspeed: sf.Scalar,
+        v_var: sf.Scalar,
+        heading_var: sf.Scalar,
+        sideslip_var: sf.Scalar,
+        airspeed_var: sf.Scalar,
+) -> (sf.V2, sf.M22):
 
     # Initialise wind states assuming zero side slip and horizontal flight
-    wind = geo.V2(v_local[0] - airspeed * sm.cos(heading), v_local[1] - airspeed * sm.sin(heading))
+    wind = sf.V2(v_local[0] - airspeed * sm.cos(heading), v_local[1] - airspeed * sm.sin(heading))
     # Zero sideslip, propagate the sideslip variance using partial derivatives w.r.t heading
     J = wind.jacobian([v_local[0], v_local[1], heading, heading, airspeed])
 
-    R = geo.M55()
+    R = sf.M55()
     R[0,0] = v_var
     R[1,1] = v_var
     R[2,2] = heading_var

src/lib/wind_estimator/python/generated/fuse_airspeed.h

@@ -1,6 +1,6 @@
 // -----------------------------------------------------------------------------
 // This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
+//     function/FUNCTION.h.jinja
 // Do NOT modify by hand.
 // -----------------------------------------------------------------------------
 

src/lib/wind_estimator/python/generated/fuse_airspeed.py

@@ -1,21 +1,21 @@
 # -----------------------------------------------------------------------------
 # This file was autogenerated by symforce from template:
-#     backends/python/templates/function/FUNCTION.py.jinja
+#     function/FUNCTION.py.jinja
 # Do NOT modify by hand.
 # -----------------------------------------------------------------------------
 
-import math  # pylint: disable=unused-import
-import numpy  # pylint: disable=unused-import
-import typing as T  # pylint: disable=unused-import
+# pylint: disable=too-many-locals,too-many-lines,too-many-statements,unused-argument,unused-import
 
-import sym  # pylint: disable=unused-import
+import math
+import typing as T
 
+import numpy
 
-# pylint: disable=too-many-locals,too-many-lines,too-many-statements,unused-argument
+import sym
 
 
 def fuse_airspeed(v_local, state, P, airspeed, R, epsilon):
-    # type: (T.Sequence[float], T.Sequence[float], numpy.ndarray, float, float, float) -> T.Tuple[numpy.ndarray, T.List[float], float, float]
+    # type: (numpy.ndarray, numpy.ndarray, numpy.ndarray, float, float, float) -> T.Tuple[numpy.ndarray, numpy.ndarray, float, float]
     """
     This function was autogenerated from a symbolic function. Do not modify by hand.
 
@@ -39,34 +39,51 @@ def fuse_airspeed(v_local, state, P, airspeed, R, epsilon):
     # Total ops: 56
 
     # Input arrays
+    if v_local.shape == (3,):
+        v_local = v_local.reshape((3, 1))
+    elif v_local.shape != (3, 1):
+        raise IndexError(
+            "v_local is expected to have shape (3, 1) or (3,); instead had shape {}".format(
+                v_local.shape
+            )
+        )
+
+    if state.shape == (3,):
+        state = state.reshape((3, 1))
+    elif state.shape != (3, 1):
+        raise IndexError(
+            "state is expected to have shape (3, 1) or (3,); instead had shape {}".format(
+                state.shape
+            )
+        )
 
     # Intermediate terms (11)
-    _tmp0 = -state[0] + v_local[0]
-    _tmp1 = -state[1] + v_local[1]
-    _tmp2 = math.sqrt(_tmp0 ** 2 + _tmp1 ** 2 + epsilon + v_local[2] ** 2)
-    _tmp3 = state[2] / _tmp2
+    _tmp0 = -state[0, 0] + v_local[0, 0]
+    _tmp1 = -state[1, 0] + v_local[1, 0]
+    _tmp2 = math.sqrt(_tmp0**2 + _tmp1**2 + epsilon + v_local[2, 0] ** 2)
+    _tmp3 = state[2, 0] / _tmp2
     _tmp4 = _tmp0 * _tmp3
     _tmp5 = _tmp1 * _tmp3
-    _tmp6 = -P[0] * _tmp4
-    _tmp7 = -P[4] * _tmp5
-    _tmp8 = P[8] * _tmp2
+    _tmp6 = -P[0, 0] * _tmp4
+    _tmp7 = -P[1, 1] * _tmp5
+    _tmp8 = P[2, 2] * _tmp2
     _tmp9 = (
         R
-        + _tmp2 * (-P[6] * _tmp4 - P[7] * _tmp5 + _tmp8)
-        - _tmp4 * (-P[1] * _tmp5 + P[2] * _tmp2 + _tmp6)
-        - _tmp5 * (-P[3] * _tmp4 + P[5] * _tmp2 + _tmp7)
+        + _tmp2 * (-P[0, 2] * _tmp4 - P[1, 2] * _tmp5 + _tmp8)
+        - _tmp4 * (-P[1, 0] * _tmp5 + P[2, 0] * _tmp2 + _tmp6)
+        - _tmp5 * (-P[0, 1] * _tmp4 + P[2, 1] * _tmp2 + _tmp7)
     )
-    _tmp10 = max(_tmp9, epsilon) ** (-1)
+    _tmp10 = 1 / max(_tmp9, epsilon)
 
     # Output terms
-    _H = numpy.zeros((1, 3))
-    _H[0, 0] = -_tmp4
-    _H[0, 1] = -_tmp5
-    _H[0, 2] = _tmp2
-    _K = [0.0] * 3
-    _K[0] = _tmp10 * (-P[3] * _tmp5 + P[6] * _tmp2 + _tmp6)
-    _K[1] = _tmp10 * (-P[1] * _tmp4 + P[7] * _tmp2 + _tmp7)
-    _K[2] = _tmp10 * (-P[2] * _tmp4 - P[5] * _tmp5 + _tmp8)
+    _H = numpy.zeros(3)
+    _H[0] = -_tmp4
+    _H[1] = -_tmp5
+    _H[2] = _tmp2
+    _K = numpy.zeros(3)
+    _K[0] = _tmp10 * (-P[0, 1] * _tmp5 + P[0, 2] * _tmp2 + _tmp6)
+    _K[1] = _tmp10 * (-P[1, 0] * _tmp4 + P[1, 2] * _tmp2 + _tmp7)
+    _K[2] = _tmp10 * (-P[2, 0] * _tmp4 - P[2, 1] * _tmp5 + _tmp8)
     _innov_var = _tmp9
-    _innov = -_tmp2 * state[2] + airspeed
+    _innov = -_tmp2 * state[2, 0] + airspeed
     return _H, _K, _innov_var, _innov

src/lib/wind_estimator/python/generated/fuse_beta.h

@@ -1,6 +1,6 @@
 // -----------------------------------------------------------------------------
 // This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
+//     function/FUNCTION.h.jinja
 // Do NOT modify by hand.
 // -----------------------------------------------------------------------------
 

src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h

@@ -1,6 +1,6 @@
 // -----------------------------------------------------------------------------
 // This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
+//     function/FUNCTION.h.jinja
 // Do NOT modify by hand.
 // -----------------------------------------------------------------------------
 
@@ -36,6 +36,9 @@ void InitWindUsingAirspeed(const matrix::Matrix<Scalar, 3, 1>& v_local, const Sc
                            matrix::Matrix<Scalar, 2, 2>* const P = nullptr) {
   // Total ops: 22
 
+  // Unused inputs
+  (void)heading_var;
+
   // Input arrays
 
   // Intermediate terms (7)

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -33,6 +33,9 @@
 Description:
 """
 
+import symforce
+symforce.set_epsilon_to_symbol()
+
 import symforce.symbolic as sf
 from derivation_utils import *
 
@@ -413,16 +416,17 @@ def predict_drag(
         cd: sf.Scalar,
         cm: sf.Scalar,
         epsilon: sf.Scalar
-        ):
+) -> (sf.Scalar):
     R_to_body = state_to_rot3(state).inverse()
 
     vel_rel = sf.V3(state[State.vx] - state[State.wx],
                     state[State.vy] - state[State.wy],
                     state[State.vz])
     vel_rel_body = R_to_body * vel_rel
+    vel_rel_body_xy = sf.V2(vel_rel_body[0], vel_rel_body[1])
 
-    bluff_body_drag = -0.5 * rho * cd * sf.V2(vel_rel_body) * vel_rel_body.norm(epsilon=epsilon)
-    momentum_drag = -cm * sf.V2(vel_rel_body)
+    bluff_body_drag = -0.5 * rho * cd * vel_rel_body_xy * vel_rel_body.norm(epsilon=epsilon)
+    momentum_drag = -cm * vel_rel_body_xy
 
     return bluff_body_drag + momentum_drag
 
@@ -435,7 +439,7 @@ def compute_drag_x_innov_var_and_k(
         cm: sf.Scalar,
         R: sf.Scalar,
         epsilon: sf.Scalar
-) -> (sf.Scalar, sf.Scalar, VState):
+) -> (sf.Scalar, VState):
 
     meas_pred = predict_drag(state, rho, cd, cm, epsilon)
     Hx = sf.V1(meas_pred[0]).jacobian(state)
@@ -455,7 +459,7 @@ def compute_drag_y_innov_var_and_k(
         cm: sf.Scalar,
         R: sf.Scalar,
         epsilon: sf.Scalar
-) -> (sf.Scalar, sf.Scalar, VState):
+) -> (sf.Scalar, VState):
 
     meas_pred = predict_drag(state, rho, cd, cm, epsilon)
     Hy = sf.V1(meas_pred[1]).jacobian(state)

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h

@@ -1,6 +1,6 @@
 // -----------------------------------------------------------------------------
 // This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
+//     function/FUNCTION.h.jinja
 // Do NOT modify by hand.
 // -----------------------------------------------------------------------------
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h

@@ -1,6 +1,6 @@
 // -----------------------------------------------------------------------------
 // This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
+//     function/FUNCTION.h.jinja
 // Do NOT modify by hand.
 // -----------------------------------------------------------------------------
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h

@@ -1,6 +1,6 @@
 // -----------------------------------------------------------------------------
 // This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
+//     function/FUNCTION.h.jinja
 // Do NOT modify by hand.
 // -----------------------------------------------------------------------------
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h

@@ -1,6 +1,6 @@
 // -----------------------------------------------------------------------------
 // This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
+//     function/FUNCTION.h.jinja
 // Do NOT modify by hand.
 // -----------------------------------------------------------------------------
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h

@@ -1,6 +1,6 @@
 // -----------------------------------------------------------------------------
 // This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
+//     function/FUNCTION.h.jinja
 // Do NOT modify by hand.
 // -----------------------------------------------------------------------------
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h

@@ -1,6 +1,6 @@
 // -----------------------------------------------------------------------------
 // This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
+//     function/FUNCTION.h.jinja
 // Do NOT modify by hand.
 // -----------------------------------------------------------------------------
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h

@@ -1,6 +1,6 @@
 // -----------------------------------------------------------------------------
 // This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
+//     function/FUNCTION.h.jinja
 // Do NOT modify by hand.
 // -----------------------------------------------------------------------------
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h

@@ -1,6 +1,6 @@
 // -----------------------------------------------------------------------------
 // This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
+//     function/FUNCTION.h.jinja
 // Do NOT modify by hand.
 // -----------------------------------------------------------------------------
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h

@@ -1,6 +1,6 @@
 // -----------------------------------------------------------------------------
 // This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
+//     function/FUNCTION.h.jinja
 // Do NOT modify by hand.
 // -----------------------------------------------------------------------------
 

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h

@@ -1,6 +1,6 @@
 // -----------------------------------------------------------------------------
 // This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
+//     function/FUNCTION.h.jinja
 // Do NOT modify by hand.
 // -----------------------------------------------------------------------------
 
@@ -37,6 +37,9 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
                                   matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
   // Total ops: 471
 
+  // Unused inputs
+  (void)epsilon;
+
   // Input arrays
 
   // Intermediate terms (49)