From d366b2147516ceba3d9e82dd2b0acb7fc6e3b0ab Mon Sep 17 00:00:00 2001 From: Ewoud Smeur Date: Tue, 31 Oct 2017 00:25:36 +0100 Subject: [PATCH] math functions and high pass filter (#2142) * math functions and high pass filter high pass filter uses doubles to avoid numerical errors * determinant check Also reverse order such that output comes first in the argument list * removed function prototypes --- sw/airborne/filters/high_pass_filter.h | 108 +++++++++++++++ sw/airborne/math/pprz_algebra_float.c | 177 ++++++++++++++++++++----- sw/airborne/math/pprz_algebra_float.h | 10 +- tests/math/test_pprz_math.c | 17 ++- 4 files changed, 273 insertions(+), 39 deletions(-) create mode 100644 sw/airborne/filters/high_pass_filter.h diff --git a/sw/airborne/filters/high_pass_filter.h b/sw/airborne/filters/high_pass_filter.h new file mode 100644 index 0000000000..cd04d37e5b --- /dev/null +++ b/sw/airborne/filters/high_pass_filter.h @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2017 Ewoud Smeur + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file filters/high_pass_filter.h + * @brief Simple high pass filter with double precision + * + */ + +#ifndef HIGH_PASS_FILTER_H +#define HIGH_PASS_FILTER_H + +#include "std.h" +#include "math/pprz_algebra_int.h" + + +/** Fourth order filter structure. + * + * Polynomial discrete form: + * + * b0 + b1 z^-1 + b2 z^-2 etc + * H(z) = ---------------------- + * a0 + a1 z^-1 + a2 z^-2 etc + */ +struct FourthOrderHighPass { + double a[4]; ///< denominator gains + double b[4]; ///< numerator gains + double i[4]; ///< input history + double o[4]; ///< output history +}; + + +/** Init fourth order high pass filter. + * + * @param filter fourth order high pass filter structure + * @param value initial value of the filter + */ +static inline void init_fourth_order_high_pass(struct FourthOrderHighPass *filter, double *a, double *b, double value) +{ + filter->a[0] = a[0]; + filter->a[1] = a[1]; + filter->a[2] = a[2]; + filter->a[3] = a[3]; + filter->b[0] = b[0]; + filter->b[1] = b[1]; + filter->b[2] = b[2]; + filter->b[3] = b[3]; + filter->i[0] = filter->i[1] = filter->i[2] = filter->i[3] = filter->o[0] = filter->o[1] = filter->o[2] = filter->o[3] = value; +} + +/** Update fourth order high pass filter state with a new value. + * + * @param filter fourth order high pass filter structure + * @param value new input value of the filter + * @return new filtered value + */ +static inline double update_fourth_order_high_pass(struct FourthOrderHighPass *filter, double value) +{ + double out = filter->b[0] * value + + filter->b[1] * filter->i[0] + + filter->b[2] * filter->i[1] + + filter->b[3] * filter->i[2] + + filter->b[0] * filter->i[3] + - filter->a[0] * filter->o[0] + - filter->a[1] * filter->o[1] + - filter->a[2] * filter->o[2] + - filter->a[3] * filter->o[3]; + + filter->i[3] = filter->i[2]; + filter->i[2] = filter->i[1]; + filter->i[1] = filter->i[0]; + filter->i[0] = value; + filter->o[3] = filter->o[2]; + filter->o[2] = filter->o[1]; + filter->o[1] = filter->o[0]; + filter->o[0] = out; + return out; +} + +/** Get current value of the fourth order high pass filter. + * + * @param filter fourth order high pass filter structure + * @return current value of the filter + */ +static inline double get_fourth_order_high_pass(struct FourthOrderHighPass *filter) +{ + return filter->o[0]; +} + +#endif + diff --git a/sw/airborne/math/pprz_algebra_float.c b/sw/airborne/math/pprz_algebra_float.c index 17c2778416..77f72c2f03 100644 --- a/sw/airborne/math/pprz_algebra_float.c +++ b/sw/airborne/math/pprz_algebra_float.c @@ -468,6 +468,12 @@ void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, qd->qz = -0.5 * (-r->r * q->qi - r->q * q->qx + r->p * q->qy + c * q->qz); } +/** + * @brief quat of euler roation 'ZYX' + * + * @param q Quat output + * @param e Euler input + */ void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e) { @@ -488,6 +494,33 @@ void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e) q->qz = c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2; } +/** + * @brief quat from euler rotation 'ZXY' + * This rotation order is useful if you need 90 deg pitch + * + * @param q Quat output + * @param e Euler input + */ +void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e) +{ + const float phi2 = e->phi / 2.0; + const float theta2 = e->theta / 2.0; + const float psi2 = e->psi / 2.0; + + const float s_phi2 = sinf(phi2); + const float c_phi2 = cosf(phi2); + const float s_theta2 = sinf(theta2); + const float c_theta2 = cosf(theta2); + const float s_psi2 = sinf(psi2); + const float c_psi2 = cosf(psi2); + + q->qi = c_phi2 * c_theta2 * c_psi2 - s_phi2 * s_theta2 * s_psi2; + q->qx = s_phi2 * c_theta2 * c_psi2 - c_phi2 * s_theta2 * s_psi2; + q->qy = c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2; + q->qz = s_phi2 * s_theta2 * c_psi2 + c_phi2 * c_theta2 * s_psi2; +} + + void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle) { const float san = sinf(angle / 2.); @@ -577,6 +610,12 @@ void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm) e->psi = atan2f(dcm01, dcm00); } +/** + * @brief euler rotation 'ZYX' + * + * @param e Euler output + * @param q Quat input + */ void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q) { const float qx2 = q->qx * q->qx; @@ -599,48 +638,110 @@ void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q) e->psi = atan2f(dcm01, dcm00); } +/** + * @brief euler rotation 'ZXY' + * This rotation order is useful if you need 90 deg pitch + * + * @param e Euler output + * @param q Quat input + */ +void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q) +{ + const float qx2 = q->qx * q->qx; + const float qy2 = q->qy * q->qy; + const float qz2 = q->qz * q->qz; + const float qi2 = q->qi * q->qi; + const float qiqx = q->qi * q->qx; + const float qiqy = q->qi * q->qy; + const float qiqz = q->qi * q->qz; + const float qxqy = q->qx * q->qy; + const float qxqz = q->qx * q->qz; + const float qyqz = q->qy * q->qz; + const float r11 = -2 * (qxqy - qiqz); + const float r12 = qi2 - qx2 + qy2 - qz2; + const float r21 = 2 * (qyqz + qiqx); + const float r31 = -2 * (qxqz - qiqy); + const float r32 = qi2 - qx2 - qy2 + qz2; + + e->psi = atan2f(r11, r12); + e->phi = asinf(r21); + e->theta = atan2f(r31, r32); +} + +/** + * @brief 2x2 matrix inverse + * + * @param inv_out[4] inverted matrix output + * @param mat_in[4] matrix to be inverted + * + * @return success (0) or not invertible (1) + */ +bool float_mat_inv_2d(float inv_out[4], float mat_in[4]) +{ + float det = mat_in[0] * mat_in[3] - mat_in[1] * mat_in[2]; + + if (fabsf(det) < 1e-4) { return 1; } //not invertible + + inv_out[0] = mat_in[3] / det; + inv_out[1] = -mat_in[1] / det; + inv_out[2] = -mat_in[2] / det; + inv_out[3] = mat_in[0] / det; + + return 0; //return success +} + +/** + * @brief Multiply 2D matrix with vector + * + * @param vect_out output vector + * @param mat[4] Matrix input + * @param vect_in Vector input + */ +void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in) +{ + vect_out->x = mat[0] * vect_in.x + mat[1] * vect_in.y; + vect_out->y = mat[2] * vect_in.x + mat[3] * vect_in.y; +} + /* * 4x4 Matrix inverse. * obtained from: http://rodolphe-vaillant.fr/?e=7 */ -float float_mat_minor_4d(float m[16], int r0, int r1, int r2, int c0, int c1, int c2); -void float_mat_adjoint_4d(float m[16], float adjOut[16]); -float float_mat_det_4d(float m[16]); -float float_mat_minor_4d(float m[16], int r0, int r1, int r2, int c0, int c1, int c2) +static float float_mat_minor_4d(float m[16], int r0, int r1, int r2, int c0, int c1, int c2) { - return m[4*r0+c0] * (m[4*r1+c1] * m[4*r2+c2] - m[4*r2+c1] * m[4*r1+c2]) - - m[4*r0+c1] * (m[4*r1+c0] * m[4*r2+c2] - m[4*r2+c0] * m[4*r1+c2]) + - m[4*r0+c2] * (m[4*r1+c0] * m[4*r2+c1] - m[4*r2+c0] * m[4*r1+c1]); + return m[4 * r0 + c0] * (m[4 * r1 + c1] * m[4 * r2 + c2] - m[4 * r2 + c1] * m[4 * r1 + c2]) - + m[4 * r0 + c1] * (m[4 * r1 + c0] * m[4 * r2 + c2] - m[4 * r2 + c0] * m[4 * r1 + c2]) + + m[4 * r0 + c2] * (m[4 * r1 + c0] * m[4 * r2 + c1] - m[4 * r2 + c0] * m[4 * r1 + c1]); } -void float_mat_adjoint_4d(float m[16], float adjOut[16]) +static void float_mat_adjoint_4d(float adjOut[16], float m[16]) { - adjOut[ 0] = float_mat_minor_4d(m,1,2,3,1,2,3); - adjOut[ 1] = -float_mat_minor_4d(m,0,2,3,1,2,3); - adjOut[ 2] = float_mat_minor_4d(m,0,1,3,1,2,3); - adjOut[ 3] = -float_mat_minor_4d(m,0,1,2,1,2,3); - adjOut[ 4] = -float_mat_minor_4d(m,1,2,3,0,2,3); - adjOut[ 5] = float_mat_minor_4d(m,0,2,3,0,2,3); - adjOut[ 6] = -float_mat_minor_4d(m,0,1,3,0,2,3); - adjOut[ 7] = float_mat_minor_4d(m,0,1,2,0,2,3); - adjOut[ 8] = float_mat_minor_4d(m,1,2,3,0,1,3); - adjOut[ 9] = -float_mat_minor_4d(m,0,2,3,0,1,3); - adjOut[10] = float_mat_minor_4d(m,0,1,3,0,1,3); - adjOut[11] = -float_mat_minor_4d(m,0,1,2,0,1,3); - adjOut[12] = -float_mat_minor_4d(m,1,2,3,0,1,2); - adjOut[13] = float_mat_minor_4d(m,0,2,3,0,1,2); - adjOut[14] = -float_mat_minor_4d(m,0,1,3,0,1,2); - adjOut[15] = float_mat_minor_4d(m,0,1,2,0,1,2); + adjOut[ 0] = float_mat_minor_4d(m, 1, 2, 3, 1, 2, 3); + adjOut[ 1] = -float_mat_minor_4d(m, 0, 2, 3, 1, 2, 3); + adjOut[ 2] = float_mat_minor_4d(m, 0, 1, 3, 1, 2, 3); + adjOut[ 3] = -float_mat_minor_4d(m, 0, 1, 2, 1, 2, 3); + adjOut[ 4] = -float_mat_minor_4d(m, 1, 2, 3, 0, 2, 3); + adjOut[ 5] = float_mat_minor_4d(m, 0, 2, 3, 0, 2, 3); + adjOut[ 6] = -float_mat_minor_4d(m, 0, 1, 3, 0, 2, 3); + adjOut[ 7] = float_mat_minor_4d(m, 0, 1, 2, 0, 2, 3); + adjOut[ 8] = float_mat_minor_4d(m, 1, 2, 3, 0, 1, 3); + adjOut[ 9] = -float_mat_minor_4d(m, 0, 2, 3, 0, 1, 3); + adjOut[10] = float_mat_minor_4d(m, 0, 1, 3, 0, 1, 3); + adjOut[11] = -float_mat_minor_4d(m, 0, 1, 2, 0, 1, 3); + adjOut[12] = -float_mat_minor_4d(m, 1, 2, 3, 0, 1, 2); + adjOut[13] = float_mat_minor_4d(m, 0, 2, 3, 0, 1, 2); + adjOut[14] = -float_mat_minor_4d(m, 0, 1, 3, 0, 1, 2); + adjOut[15] = float_mat_minor_4d(m, 0, 1, 2, 0, 1, 2); } -float float_mat_det_4d(float m[16]) +static float float_mat_det_4d(float m[16]) { - return m[0] * float_mat_minor_4d(m, 1, 2, 3, 1, 2, 3) - - m[1] * float_mat_minor_4d(m, 1, 2, 3, 0, 2, 3) + - m[2] * float_mat_minor_4d(m, 1, 2, 3, 0, 1, 3) - - m[3] * float_mat_minor_4d(m, 1, 2, 3, 0, 1, 2); + return m[0] * float_mat_minor_4d(m, 1, 2, 3, 1, 2, 3) - + m[1] * float_mat_minor_4d(m, 1, 2, 3, 0, 2, 3) + + m[2] * float_mat_minor_4d(m, 1, 2, 3, 0, 1, 3) - + m[3] * float_mat_minor_4d(m, 1, 2, 3, 0, 1, 2); } /** @@ -649,12 +750,18 @@ float float_mat_det_4d(float m[16]) * @param invOut output array, inverse of mat_in * @param mat_in input array */ -void float_mat_inv_4d(float invOut[16], float mat_in[16]) +bool float_mat_inv_4d(float invOut[16], float mat_in[16]) { - float_mat_adjoint_4d(mat_in, invOut); + float_mat_adjoint_4d(invOut, mat_in); - float inv_det = 1.0f / float_mat_det_4d(mat_in); - int i; - for(i = 0; i < 16; ++i) - invOut[i] = invOut[i] * inv_det; + float det = float_mat_det_4d(mat_in); + if (fabsf(det) < 1e-4) { return 1; } //not invertible + + float inv_det = 1.0f / det; + int i; + for (i = 0; i < 16; ++i) { + invOut[i] = invOut[i] * inv_det; + } + + return 0; //success } diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 14ec31b9ec..cb46919204 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -279,13 +279,13 @@ extern void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b * rb = m_a2b * ra */ extern void float_rmat_mult(struct FloatEulers *rb, struct FloatRMat *m_a2b, - struct FloatEulers *ra); + struct FloatEulers *ra); /** rotate angle by transposed rotation matrix. * rb = m_b2a^T * ra */ extern void float_rmat_transp_mult(struct FloatEulers *rb, struct FloatRMat *m_b2a, - struct FloatEulers *ra); + struct FloatEulers *ra); /** rotate anglular rates by rotation matrix. * rb = m_a2b * ra @@ -450,6 +450,7 @@ extern void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, cons /// Quaternion from Euler angles. extern void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e); +extern void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e); /// Quaternion from unit vector and angle. extern void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle); @@ -502,6 +503,7 @@ static inline float float_eulers_norm(struct FloatEulers *e) } extern void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm); extern void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q); +extern void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q); /* defines for backwards compatibility */ #define FLOAT_EULERS_OF_RMAT(_e, _rm) WARNING("FLOAT_EULERS_OF_RMAT macro is deprecated, use the lower case function instead") float_eulers_of_rmat(&(_e), &(_rm)) @@ -727,7 +729,9 @@ static inline void float_mat_col(float *o, float **a, int m, int c) } } -extern void float_mat_inv_4d(float invOut[16], float mat_in[16]); +extern bool float_mat_inv_2d(float inv_out[4], float mat_in[4]); +extern void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in); +extern bool float_mat_inv_4d(float invOut[16], float mat_in[16]); #ifdef __cplusplus } /* extern "C" */ diff --git a/tests/math/test_pprz_math.c b/tests/math/test_pprz_math.c index 1a4548555e..dc8476c143 100644 --- a/tests/math/test_pprz_math.c +++ b/tests/math/test_pprz_math.c @@ -29,11 +29,12 @@ #include "tap.h" #include "math/pprz_algebra_int.h" +#include "math/pprz_algebra_float.h" int main() { note("running algebra math tests"); - plan(1); + plan(3); /* test int32_vect2_normalize */ struct Int32Vect2 v = {2300, -4200}; @@ -42,6 +43,20 @@ int main() ok((v.x == 491 && v.y == -898), "int32_vect2_normalize([2300, -4200], 10) returned [%d, %d]", v.x, v.y); + /*test float_eulers_of_quat_zxy*/ + struct FloatQuat quat = {0.9266, -0.2317, 0.1165, 0.2722}; + struct FloatEulers eulers_zxy; + float_eulers_of_quat_zxy(&eulers_zxy, &quat); + + ok((fabs(eulers_zxy.psi - 0.6436) < 0.01 && fabs(eulers_zxy.phi - -0.3746) < 0.01 && fabs(eulers_zxy.theta - 0.3763) < 0.01), + "float_eulers_of_quat_zxy(0.9266, -0.2317, 0.1165, 0.2722) returned [%f, %f, %f]", eulers_zxy.phi, eulers_zxy.theta, eulers_zxy.psi); + + /*test float_quat_of_eulers_zxy*/ + struct FloatQuat quat_zxy; + float_quat_of_eulers_zxy(&quat_zxy, &eulers_zxy); + ok((fabs(quat_zxy.qi - 0.9266) < 0.01 && fabs(quat_zxy.qx - -0.2317) < 0.01 && fabs(quat_zxy.qy - 0.1165) < 0.01) && fabs(quat_zxy.qz - 0.2722), + "float_quat_of_eulers_zxy(float_eulers_of_quat_zxy(0.9266, -0.2317, 0.1165, 0.2722)) returned [%f, %f, %f, %f]", quat_zxy.qi, quat_zxy.qx, quat_zxy.qy, quat_zxy.qz); + done_testing(); }