mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 14:18:00 +08:00
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
This commit is contained in:
Ewoud Smeur
committed by
Gautier Hattenberger
Gautier Hattenberger
parent
3ff008f62e
commit
d366b21475
@@ -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
|
||||
|
||||
@@ -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
|
||||
}
|
||||
|
||||
@@ -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" */
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user