From c99f56f01055640421b0eca53a8443bc353531fb Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 26 Jun 2023 15:12:42 +0200 Subject: [PATCH] wind_estimator: update generated code to Symforce 0.9.0 --- .../python/generated/fuse_airspeed.h | 2 +- .../python/generated/fuse_airspeed.py | 71 ++++++++++++------- .../python/generated/fuse_beta.h | 2 +- .../generated/init_wind_using_airspeed.h | 5 +- 4 files changed, 50 insertions(+), 30 deletions(-) diff --git a/src/lib/wind_estimator/python/generated/fuse_airspeed.h b/src/lib/wind_estimator/python/generated/fuse_airspeed.h index a3d677736d..5d1e293516 100644 --- a/src/lib/wind_estimator/python/generated/fuse_airspeed.h +++ b/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. // ----------------------------------------------------------------------------- diff --git a/src/lib/wind_estimator/python/generated/fuse_airspeed.py b/src/lib/wind_estimator/python/generated/fuse_airspeed.py index 3b6180320f..cdbdc597b1 100644 --- a/src/lib/wind_estimator/python/generated/fuse_airspeed.py +++ b/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 diff --git a/src/lib/wind_estimator/python/generated/fuse_beta.h b/src/lib/wind_estimator/python/generated/fuse_beta.h index 39469ce892..cbfde79770 100644 --- a/src/lib/wind_estimator/python/generated/fuse_beta.h +++ b/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. // ----------------------------------------------------------------------------- diff --git a/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h b/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h index f79f656009..cbfa6fa152 100644 --- a/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h +++ b/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& v_local, const Sc matrix::Matrix* const P = nullptr) { // Total ops: 22 + // Unused inputs + (void)heading_var; + // Input arrays // Intermediate terms (7)