From 89bc28e836075f9c30aa4250e2a434b21cc356ec Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 22 Sep 2022 15:59:10 +0200 Subject: [PATCH] wind_est: extract utility functions to separate file --- src/lib/wind_estimator/python/derivation.py | 120 +++++++----------- .../wind_estimator/python/derivation_utils.py | 101 +++++++++++++++ 2 files changed, 149 insertions(+), 72 deletions(-) create mode 100644 src/lib/wind_estimator/python/derivation_utils.py diff --git a/src/lib/wind_estimator/python/derivation.py b/src/lib/wind_estimator/python/derivation.py index 04a2b06376..f3d844e266 100644 --- a/src/lib/wind_estimator/python/derivation.py +++ b/src/lib/wind_estimator/python/derivation.py @@ -1,7 +1,43 @@ -import os +#!/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.py +Description: + 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 +from derivation_utils import * def fuse_airspeed( v_local: geo.V3, @@ -18,36 +54,12 @@ def fuse_airspeed( innov = airspeed - airspeed_pred H = geo.V1(airspeed_pred).jacobian(state) - innov_var = (H * P * H.transpose() + R)[0,0] + innov_var = (H * P * H.T + R)[0,0] - K = P * H.transpose() / sm.Max(innov_var, epsilon) + K = P * H.T / sm.Max(innov_var, epsilon) return (geo.V3(H), K, innov_var, innov) -# q: quaternion describing rotation from frame 1 to frame 2 -# returns a rotation matrix derived form q which describes the same -# rotation -def quat2Rot(q): - q0 = q[0] - q1 = q[1] - q2 = q[2] - q3 = q[3] - - Rot = geo.M33([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)], - [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)], - [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]]) - - return Rot - -def sign_no_zero(x) -> T.Scalar: - """ - Returns -1 if x is negative, 1 if x is positive, and 1 if x is zero - """ - return 2 * sm.Min(sm.sign(x), 0) + 1 - -def add_epsilon_sign(expr, var, eps): - return expr.subs(var, var + eps * sign_no_zero(var)) - def fuse_beta( v_local: geo.V3, state: geo.V3, @@ -58,18 +70,18 @@ def fuse_beta( ) -> (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]) - relative_wind_body = quat2Rot(q_att).T * vel_rel + relative_wind_body = quat_to_rot(q_att).T * vel_rel - # small angle approximation of side slip model - # protect division by zero using epsilon + # Small angle approximation of side slip model + # Protect division by zero using epsilon beta_pred = add_epsilon_sign(relative_wind_body[1] / relative_wind_body[0], relative_wind_body[0], epsilon) innov = 0.0 - beta_pred H = geo.V1(beta_pred).jacobian(state) - innov_var = (H * P * H.transpose() + R)[0,0] + innov_var = (H * P * H.T + R)[0,0] - K = P * H.transpose() / sm.Max(innov_var, epsilon) + K = P * H.T / sm.Max(innov_var, epsilon) return (geo.V3(H), K, innov_var, innov) @@ -99,44 +111,8 @@ def init_wind_using_airspeed( return (wind, P) -def generatePx4Function(function_name, output_names): - from symforce.codegen import Codegen, CppConfig - codegen = Codegen.function( - function_name, - output_names=output_names, - config=CppConfig()) - metadata = codegen.generate_function( - output_dir="generated", - skip_directory_nesting=True) +generate_px4_function(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"]) +generate_px4_function(fuse_beta, output_names=["H", "K", "innov_var", "innov"]) +generate_px4_function(init_wind_using_airspeed, output_names=["wind", "P"]) - print("Files generated in {}:\n".format(metadata.output_dir)) - for f in metadata.generated_files: - print(" |- {}".format(os.path.relpath(f, metadata.output_dir))) - - # Replace cstdlib and Eigen functions by PX4 equivalents - import fileinput - - with fileinput.FileInput(os.path.abspath(metadata.generated_files[0]), inplace=True, backup='.bak') as file: - for line in file: - line = line.replace("std::max", "math::max") - line = line.replace("std::min", "math::min") - line = line.replace("Eigen", "matrix") - line = line.replace("matrix/Dense", "matrix/math.hpp") - print(line, end='') - -def generatePythonFunction(function_name, output_names): - from symforce.codegen import Codegen, PythonConfig - codegen = Codegen.function( - function_name, - output_names=output_names, - config=PythonConfig()) - - metadata = codegen.generate_function( - output_dir="generated", - skip_directory_nesting=True) - -generatePx4Function(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"]) -generatePx4Function(fuse_beta, output_names=["H", "K", "innov_var", "innov"]) -generatePx4Function(init_wind_using_airspeed, output_names=["wind", "P"]) - -generatePythonFunction(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"]) +generate_python_function(fuse_airspeed, output_names=["H", "K", "innov_var", "innov"]) diff --git a/src/lib/wind_estimator/python/derivation_utils.py b/src/lib/wind_estimator/python/derivation_utils.py new file mode 100644 index 0000000000..68ee60931e --- /dev/null +++ b/src/lib/wind_estimator/python/derivation_utils.py @@ -0,0 +1,101 @@ +#!/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_utils.py +Description: + Common functions used for the derivation of most estimators +""" + +from symforce import symbolic as sm +from symforce import geo +from symforce import typing as T + +# q: quaternion describing rotation from frame 1 to frame 2 +# returns a rotation matrix derived form q which describes the same +# rotation +def quat_to_rot(q): + q0 = q[0] + q1 = q[1] + q2 = q[2] + q3 = q[3] + + Rot = geo.M33([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)], + [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)], + [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]]) + + return Rot + +def sign_no_zero(x) -> T.Scalar: + """ + Returns -1 if x is negative, 1 if x is positive, and 1 if x is zero + """ + return 2 * sm.Min(sm.sign(x), 0) + 1 + +def add_epsilon_sign(expr, var, eps): + # Avoids a singularity at 0 while keeping the derivative correct + return expr.subs(var, var + eps * sign_no_zero(var)) + +def generate_px4_function(function_name, output_names): + from symforce.codegen import Codegen, CppConfig + import os + import fileinput + + codegen = Codegen.function( + function_name, + output_names=output_names, + config=CppConfig()) + metadata = codegen.generate_function( + output_dir="generated", + skip_directory_nesting=True) + + print("Files generated in {}:\n".format(metadata.output_dir)) + for f in metadata.generated_files: + print(" |- {}".format(os.path.relpath(f, metadata.output_dir))) + + # Replace cstdlib and Eigen functions by PX4 equivalents + with fileinput.FileInput(os.path.abspath(metadata.generated_files[0]), inplace=True, backup='.bak') as file: + for line in file: + line = line.replace("std::max", "math::max") + line = line.replace("std::min", "math::min") + line = line.replace("Eigen", "matrix") + line = line.replace("matrix/Dense", "matrix/math.hpp") + print(line, end='') + +def generate_python_function(function_name, output_names): + from symforce.codegen import Codegen, PythonConfig + codegen = Codegen.function( + function_name, + output_names=output_names, + config=PythonConfig()) + + metadata = codegen.generate_function( + output_dir="generated", + skip_directory_nesting=True)