From 4d21c90cbbdeefa34167fa04bf501b2f04f6025c Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Thu, 11 Aug 2022 18:53:49 +0200
Subject: [PATCH] wind_est: generate wind estimator equations for python use

---
 src/lib/wind_estimator/python/derivation.py   | 12 +++-
 .../python/generated/fuse_airspeed.py         | 72 +++++++++++++++++++
 2 files changed, 83 insertions(+), 1 deletion(-)
 create mode 100644 src/lib/wind_estimator/python/generated/fuse_airspeed.py

diff --git a/src/lib/wind_estimator/python/derivation.py b/src/lib/wind_estimator/python/derivation.py
index c5948eb422..c204ab18db 100644
--- a/src/lib/wind_estimator/python/derivation.py
+++ b/src/lib/wind_estimator/python/derivation.py
@@ -10,7 +10,7 @@ def fuse_airspeed(
         airspeed: T.Scalar,
         R: T.Scalar,
         epsilon: T.Scalar
-) -> geo.V3:
+) -> (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])
     airspeed_pred = vel_rel.norm(epsilon=epsilon) * state[2]
@@ -47,3 +47,13 @@ with fileinput.FileInput(os.path.abspath(metadata.generated_files[0]), inplace=T
         line = line.replace("Eigen", "matrix")
         line = line.replace("matrix/Dense", "matrix/math.hpp")
         print(line, end='')
+
+# Generate python code
+codegen = Codegen.function(
+        fuse_airspeed,
+        output_names=["H", "K", "innov_var", "innov"],
+        config=PythonConfig())
+
+metadata = codegen.generate_function(
+        output_dir="generated",
+        skip_directory_nesting=True)
diff --git a/src/lib/wind_estimator/python/generated/fuse_airspeed.py b/src/lib/wind_estimator/python/generated/fuse_airspeed.py
new file mode 100644
index 0000000000..29971c5ca6
--- /dev/null
+++ b/src/lib/wind_estimator/python/generated/fuse_airspeed.py
@@ -0,0 +1,72 @@
+# -----------------------------------------------------------------------------
+# This file was autogenerated by symforce from template:
+#     backends/python/templates/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
+
+import sym  # pylint: disable=unused-import
+
+
+# pylint: disable=too-many-locals,too-many-lines,too-many-statements,unused-argument
+
+
+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]
+    """
+    This function was autogenerated from a symbolic function. Do not modify by hand.
+
+    Symbolic function: fuse_airspeed
+
+    Args:
+        v_local: Matrix31
+        state: Matrix31
+        P: Matrix33
+        airspeed: Scalar
+        R: Scalar
+        epsilon: Scalar
+
+    Outputs:
+        H: Matrix13
+        K: Matrix31
+        innov_var: Scalar
+        innov: Scalar
+    """
+
+    # Total ops: 56
+
+    # Input arrays
+
+    # 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
+    _tmp4 = _tmp0 * _tmp3
+    _tmp5 = _tmp1 * _tmp3
+    _tmp6 = -P[0] * _tmp4
+    _tmp7 = -P[4] * _tmp5
+    _tmp8 = P[8] * _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)
+    )
+    _tmp10 = max(_tmp9, epsilon) ** (-1)
+
+    # 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)
+    _innov_var = _tmp9
+    _innov = -_tmp2 * state[2] + airspeed
+    return _H, _K, _innov_var, _innov