From 4f078b2a0f7c747d60196e495adc3d00b250ddd0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=A9r=C3=B4me?= Date: Sun, 17 Nov 2019 17:10:50 +0100 Subject: [PATCH] Added Filter 1, Filter 2. --- FlyByWire.cpp | 103 +++++++++++++++++++++ FlyByWire.hpp | 236 +++++++++++++++++++++++++++++++++++++++++++++--- main.cpp | 17 ++++ test/1_test.cpp | 65 +++++++++++++ 4 files changed, 409 insertions(+), 12 deletions(-) diff --git a/FlyByWire.cpp b/FlyByWire.cpp index e607da3..3cd3c80 100644 --- a/FlyByWire.cpp +++ b/FlyByWire.cpp @@ -130,4 +130,107 @@ Real Integrator1::Filter(Real x_n) return y_n; } +Filter1::Filter1(Real b1, Real b0, Real a1, Real a0, Real Ts, Real y0) +{ + SetState(y0); + ComputeCoeffsFromContinuousTF(b1, b0, a1, a0, Ts); +} + +void Filter1::ComputeCoeffsFromContinuousTF(Real b1, Real b0, Real a1, Real a0, Real Ts) +{ + cx_n = (2*b1 + b0*Ts)/(2*a1 + a0*Ts); // C(x[n]) + cx_n_1 = (-2*b1 + b0*Ts)/(2*a1 + a0*Ts); // C(x[n-1]) + cy_n_1 = (2*a1 - a0*Ts)/(2*a1 + a0*Ts); // C(y[n-1]) +} + +void Filter1::SetState(Real y_, Real x_) +{ + y_n_1 = y_; + x_n_1 = x_; +} + +Real Filter1::Filter(Real x_n) +{ + Real y_n = cx_n*x_n + cx_n_1*x_n_1 + cy_n_1*y_n_1; + x_n_1 = x_n; + y_n_1 = y_n; + return y_n; +} + +Filter1 Filter1::FilteredDerivative(Real Ts, Real tau) +{ + return Filter1(Real(1.), Real(0.), tau, Real(1.), Ts, Real(0.)); +} + +Filter2::Filter2(Real b2, Real b1, Real b0, Real a2, Real a1, Real a0, Real Ts, Real y0) +{ + SetState(y0); + ComputeCoeffsFromContinuousTF(b2, b1, b0, a2, a1, a0, Ts); +} + +void Filter2::ComputeCoeffsFromContinuousTF(Real b2, Real b1, Real b0, Real a2, Real a1, Real a0, Real Ts) +{ + Real Tssqr = Ts*Ts; + cx_n = (Real(4.)*b2 + Real(2.)*b1*Ts + b0*Tssqr)/(Real(4.)*a2 + Real(2.)*a1*Ts + a0*Tssqr); // C(x[n]) + cx_n_1 = (-Real(8.)*b2 + Real(2.)*b0*Tssqr)/(Real(4.)*a2 + Real(2.)*a1*Ts + a0*Tssqr); // C(x[n-1]) + cx_n_2 = (Real(4.)*b2 - Real(2.)*b1*Ts + b0*Tssqr)/(Real(4.)*a2 + Real(2.)*a1*Ts + a0*Tssqr); // C(x[n-2]) + cy_n_1 = (-Real(8.)*a2 + Real(2.)*a0*Tssqr)/(Real(4.)*a2 + Real(2.)*a1*Ts + a0*Tssqr); // C(y[n-1]) + cy_n_2 = (Real(4.)*a2 - Real(2.)*a1*Ts + a0*Tssqr)/(Real(4.)*a2 + Real(2.)*a1*Ts + a0*Tssqr); // C(y[n-2]) +} + +void Filter2::SetState(Real y_, Real x_) +{ + y_n_1 = y_; + y_n_2 = y_; + x_n_1 = x_; + x_n_2 = x_; +} + +Real Filter2::Filter(Real x_n) +{ + Real y_n = cx_n*x_n + cx_n_1*x_n_1 + cx_n_2*x_n_2 - cy_n_1*y_n_1 - cy_n_2*y_n_2; + x_n_2 = x_n_1; + x_n_1 = x_n; + y_n_2 = y_n_1; + y_n_1 = y_n; + return y_n; +} + +Filter2 Filter2::FromPolesAndZeros(Real a, Complex p1, Complex p2, Complex z1, Complex z2, Real Ts) +{ + Complex b2(a), b1(a*(-z1 - z2)), b0(a*z1*z2), + a2(Real(1.)), a1(-p1 - p2), a0(p1*p2); + + return Filter2(b2.real(), b1.real(), b0.real(), a2.real(), a1.real(), a0.real(), Ts); +} + +Filter2 Filter2::FromPoles(Real a, Complex p1, Complex p2, Real Ts) +{ + Complex b2(Real(0.)), b1(Real(0.)), b0(a*p1*p2), + a2(Real(1.)), a1(-p1 - p2), a0(p1*p2); + return Filter2(b2.real(), b1.real(), b0.real(), a2.real(), a1.real(), a0.real(), Ts); +} + +Filter2 Filter2::Lowpass(Real omega, Real Q, Real Ts) +{ + Real omegasqr = omega*omega; + return Filter2(Real(0.), Real(0.), omegasqr, Real(1.), omega/Q, omegasqr, Ts); +} + +Filter2 Filter2::Highpass(Real omega, Real Q, Real Ts) +{ + return Filter2(Real(1.), Real(0.), Real(0.), Real(1.), omega/Q, omega*omega, Ts); +} + +Filter2 Filter2::Bandpass(Real omega, Real Q, Real Ts) +{ + return Filter2(Real(0.), omega/Q, Real(0.), Real(1.), omega/Q, omega*omega, Ts); +} + +Filter2 Filter2::Bandstop(Real omega, Real Q, Real Ts) +{ + Real omegasqr = omega*omega; + return Filter2(Real(1.), Real(0.), omegasqr, Real(1.), omega/Q, omegasqr, Ts); +} + }// namespace FlyByWire diff --git a/FlyByWire.hpp b/FlyByWire.hpp index e5e579c..905dbeb 100644 --- a/FlyByWire.hpp +++ b/FlyByWire.hpp @@ -2,8 +2,7 @@ #define DEF_FlyByWire #include - -typedef double Real; +#include #define HUGE_VALUE_REAL Real(1e20) @@ -14,6 +13,9 @@ typedef double Real; namespace FlyByWire { +typedef double Real; +typedef std::complex Complex; + /// Computes the heading error using the shortest route from hdg to tgt, both expressed in radians. Real Hdg_err(Real tgt, Real hdg); @@ -52,10 +54,10 @@ class RateLimiter Real Filter(Real x_n); public: - Real dy_dt_min; //< Min rate. - Real dy_dt_max; //< Max rate. - Real dt; //< Time step. - Real y_n_1; //< Previous filter value. + Real dy_dt_min; //!< Min rate. + Real dy_dt_max; //!< Max rate. + Real dt; //!< Time step. + Real y_n_1; //!< Previous filter value. }; /// Heaviside function. 1 when x >= 0, 0 otherwise. @@ -71,10 +73,13 @@ Real Heaviside(Real x); /// By default, the integrator is not bounded. /// /// The class is to be used like so : -/// flt = Integrator1(Ts) +/// +/// flt = Integrator1(Ts) /// /// while control/filtering loop: +/// /// y_n = flt.Filter(x_n) +/// class Integrator1 { public: @@ -89,11 +94,218 @@ class Integrator1 Real Filter(Real x_n); public: - Real Ts;//< Time step. - Real lower_bound;//< Time step. - Real upper_bound;//< Time step. - Real x_n_1;//< x[n-1]. - Real y_n_1;//< y[n-1]. + Real Ts;//!< Time step. + Real lower_bound;//!< Lower bound. + Real upper_bound;//!< Upper bound. + Real x_n_1;//!< x[n-1]. + Real y_n_1;//!< y[n-1]. +}; + +/// First order discretetized filter. The coefficients are automatically computed from the continuous-time transfer function : +/// +/// Y b1 s + b0 +/// G(s) = --- = --------- +/// X a1 s + a0 +/// +/// The Laplace transfer function is first discretized using the Tustin method (sampling period Ts). +/// +/// Y -2 b1 + b0 Ts + (2 b1 + b0 Ts) z +/// G(z) = --- = -------------------------------- +/// X -2 a1 + a0 Ts + (2 a1 + a0 Ts) z +/// +/// The coefficients of the recurrence equation are then computed : +/// +/// 2 b1 + b0 Ts +/// C(x[n]) = ------------ +/// 2 a1 + a0 Ts +/// +/// +/// -2 b1 + b0 Ts +/// C(x[n-1]) = ------------ +/// 2 a1 + a0 Ts +/// +/// +/// 2 a1 - a0 Ts +/// C(y[n-1]) = ------------ +/// 2 a1 + a0 Ts +/// +/// The class is to be used like so : +/// +/// flt = filter1(b1, b0, a1, a0, Ts) +/// +/// +/// while control/filtering loop: +/// +/// y_n = flt.Filter(x_n) +/// +/// +/// The necessary previous values y[n-1] and x[n-1] are automatically stored within the object. +class Filter1 +{ + public: + /// Creates a Filter1 object from the continuous TF's coefficients. + Filter1(Real b1, Real b0, Real a1, Real a0, Real Ts, Real y0 = Real(0.)); + + /// Computes the filter coefficients from the continuous TF's coefficients. + void ComputeCoeffsFromContinuousTF(Real b1, Real b0, Real a1, Real a0, Real Ts); + + /// Sets the state of the filter so that it can be initialized at any value. + /// By default, the states are initialized to 0. + void SetState(Real y_ = Real(0.), Real x_ = Real(0.)); + + /// Computes the next output value of the filter y[n] from the next input value x[n]. + Real Filter(Real x_n); + + /// First order discretized filter that computes the derivative of the input signal. + /// tau controls how fast the filter approximates the derivative of the input. + /// A good default value is 0.01. + /// + /// s + /// G(s) = --------- + /// tau*s + 1 + /// + static Filter1 FilteredDerivative(Real Ts, Real tau = 0.01); + + public: + Real cx_n; //!< C(x[n]) + Real cx_n_1; //!< C(x[n-1]) + Real cy_n_1; //!< C(y[n-1]) + Real x_n_1; //!< x[n-1] + Real y_n_1; //!< y[n-1] +}; + +/// Second order discretetized filter. The coefficients are automatically computed from the continuous-time transfer function : +/// +/// Y b2 s^2 + b1 s + b0 +/// G(s) = --- = ------------------ +/// X a2 s^2 + a1 s + a0 +/// +/// The Laplace transfer function is first discretized using the Tustin method (sampling period Ts). +/// +/// Y 4 b2 - 2 b1 Ts + b0 Ts^2 + (-8 b2 + 2 b0 Ts^2) z + (4 b2 + 2 b1 Ts + b0 Ts^2) z^2 +/// G(z) = --- = --------------------------------------------------------------------------------- +/// X 4 a2 - 2 a1 Ts + a0 Ts^2 + (-8 a2 + 2 a0 Ts^2) z + (4 a2 + 2 a1 Ts + a0 Ts^2) z^2 +/// +/// The coefficients of the recurrence equation are then computed : +/// +/// 4 b2 + 2 b1 Ts + b0 Ts^2 +/// C(x[n]) = ------------------------ +/// 4 a2 + 2 a1 Ts + a0 Ts^2 +/// +/// +/// -8 b2 + 2 b0 Ts^2 +/// C(x[n-1]) = ------------------------ +/// 4 a2 + 2 a1 Ts + a0 Ts^2 +/// +/// +/// 4 b2 - 2 b1 Ts + b0 Ts^2 +/// C(x[n-2]) = ------------------------ +/// 4 a2 + 2 a1 Ts + a0 Ts^2 +/// +/// +/// -8 a2 + 2 a0 Ts^2 +/// C(y[n-1]) = ------------------------ +/// 4 a2 + 2 a1 Ts + a0 Ts^2 +/// +/// +/// 4 a2 - 2 a1 Ts + a0 Ts^2 +/// C(y[n-2]) = ------------------------ +/// 4 a2 + 2 a1 Ts + a0 Ts^2 +/// +/// The class is to be used like so : +/// +/// flt = filter2(b2, b1, b0, a2, a1, a0, Ts) +/// +/// while control/filtering loop: +/// +/// y_n = flt.Filter(x_n) +/// +/// The necessary previous values y[n-1], y[n-2], x[n-1], and x[n-2] are automatically stored within the object. + +class Filter2 +{ + public: + /// Creates a Filter2 object from the continuous TF's coefficients. + Filter2(Real b2, Real b1, Real b0, Real a2, Real a1, Real a0, Real Ts, Real y0 = Real(0.)); + + /// Computes the filter coefficients from the continuous TF's coefficients. + void ComputeCoeffsFromContinuousTF(Real b2, Real b1, Real b0, Real a2, Real a1, Real a0, Real Ts); + + /// Sets the state of the filter so that it can be initialized at any value. + /// By default, the states are initialized to 0. + void SetState(Real y_ = Real(0.), Real x_ = Real(0.)); + + /// Computes the next output value of the filter y[n] from the next input value x[n]. + Real Filter(Real x_n); + + /// Creates the transfer function from its poles, zeros, and a gain. + /// + /// a (s - z1) (s - z2) + /// G(s) = ------------------- + /// (s - p1) (s - p2) + /// + static Filter2 FromPolesAndZeros(Real a, Complex p1, Complex p2, Complex z1, Complex z2, Real Ts); + + /// Creates the transfer function from its poles, and the static gain a. + /// The poles can be real, or complex conjugate. + /// + /// a p1 p2 + /// G(s) = ----------------- + /// (s - p1) (s - p2) + /// + static Filter2 FromPoles(Real a, Complex p1, Complex p2, Real Ts); + + /// Second order discretetized low-pass biquadratic filter. + /// + /// The filter is parametrized using the cutoff frequency omega in rad/s, and the quality factor Q. + /// For no resonnance peak, use Q < 1/2 + /// + /// w^2 + /// G(s) = ----------------- + /// s^2 + w/Q*s + w^2 + /// + static Filter2 Lowpass(Real omega, Real Q, Real Ts); + + /// Second order discretetized high-pass biquadratic filter. + /// The filter is parametrized using the cutoff frequency omega in rad/s, and the quality factor Q. + /// For no resonnance peak, use Q < 1/2 + /// + /// s^2 + /// G(s) = ----------------- + /// s^2 + w/Q*s + w^2 + /// + static Filter2 Highpass(Real omega, Real Q, Real Ts); + + /// Second order discretetized band-pass biquadratic filter. + /// The filter is parametrized using the cutoff frequency omega in rad/s, and the quality factor Q. + /// For no resonnance peak, use Q < 1/2 + /// + /// w/Q*s + /// G(s) = ----------------- + /// s^2 + w/Q*s + w^2 + /// + static Filter2 Bandpass(Real omega, Real Q, Real Ts); + + /// Second order discretetized band-stop biquadratic filter. + /// The filter is parametrized using the cutoff frequency omega in rad/s, and the quality factor Q. + /// For no resonnance peak, use Q < 1/2 + /// + /// s^2 + w^2 + /// G(s) = ----------------- + /// s^2 + w/Q*s + w^2 + /// + static Filter2 Bandstop(Real omega, Real Q, Real Ts); + + public: + Real cx_n; //!< C(x[n]) + Real cx_n_1; //!< C(x[n-1]) + Real cx_n_2; //!< C(x[n-2]) + Real cy_n_1; //!< C(y[n-1]) + Real cy_n_2; //!< C(y[n-2]) + Real x_n_1; //!< x[n-1] + Real x_n_2; //!< y[n-2] + Real y_n_1; //!< x[n-1] + Real y_n_2; //!< y[n-2] }; } diff --git a/main.cpp b/main.cpp index 4699842..4ca436c 100644 --- a/main.cpp +++ b/main.cpp @@ -6,9 +6,26 @@ using std::cout; using std::endl; +using FlyByWire::Real; +using FlyByWire::Complex; + int main() { + cout.precision(16); + { + double tend = 10., + dt = 0.1, + t, y = 0.; + FlyByWire::Filter1 filt1(1, 2, 3, 4, dt, y); + + for(int i = 0 ; i <= (tend/dt) ; i++) + { + t = i*dt; + cout << t << " " << y << "\n"; + y = filt1.Filter(1.); + } + } return 0; } diff --git a/test/1_test.cpp b/test/1_test.cpp index 93fb71c..5a87c73 100644 --- a/test/1_test.cpp +++ b/test/1_test.cpp @@ -9,6 +9,9 @@ #define printvec(x) PRINT_VEC(x); #define printstr(x) PRINT_STR(x); +using FlyByWire::Real; +using FlyByWire::Complex; + const Real deg = M_PI/180.; const Real tol = 1e-12; @@ -110,3 +113,65 @@ TEST_CASE( "Integrator1", "[Integrator1]" ) REQUIRE(check_almost_equal(v_int, lower_bound, tol)); } } + +TEST_CASE( "Filter1", "[Filter1]" ) +{ + std::cout.precision(16); + + SECTION( "Filter coefficient computation" ) + { + Real b1 = 1, b0 = 2, + a1 = 3, a0 = 4, + dt = 0.1; + FlyByWire::Filter1 filt1(b1, b0, a1, a0, dt); + REQUIRE(check_almost_equal(filt1.cx_n, 0.343750000000000, tol)); + REQUIRE(check_almost_equal(filt1.cx_n_1, -0.281250000000000, tol)); + REQUIRE(check_almost_equal(filt1.cy_n_1, 0.875000000000000, tol)); + } +} + +TEST_CASE( "Filter2", "[Filter2]" ) +{ + std::cout.precision(16); + + SECTION( "Filter coefficient computation" ) + { + Real b2 = 1., b1 = 2., b0 = 3., + a2 = 4., a1 = 5., a0 = 6., + dt = 0.1; + FlyByWire::Filter2 filt2(b2, b1, b0, a2, a1, a0, dt); + REQUIRE(check_almost_equal(filt2.cx_n, 0.259671746776084, tol)); + REQUIRE(check_almost_equal(filt2.cx_n_1, -0.465416178194607, tol)); + REQUIRE(check_almost_equal(filt2.cx_n_2, 0.212778429073857, tol)); + REQUIRE(check_almost_equal(filt2.cy_n_1, -1.868698710433763, tol)); + REQUIRE(check_almost_equal(filt2.cy_n_2, 0.882766705744432, tol)); + } + + SECTION( "Filter coefficient computation from poles and zeros" ) + { + Real a = 1.5, + dt = 0.1; + Complex p1(-1,2), p2(-1,-2), z1(-2,1), z2(-2,-1); + FlyByWire::Filter2 filt2 = FlyByWire::Filter2::FromPolesAndZeros(a, p1, p2, z1, z2, dt); + REQUIRE(check_almost_equal(filt2.cx_n, 1.634831460674157, tol)); + REQUIRE(check_almost_equal(filt2.cx_n_1, -2.6629213483146064, tol)); + REQUIRE(check_almost_equal(filt2.cx_n_2, 1.095505617977528, tol)); + REQUIRE(check_almost_equal(filt2.cy_n_1, -1.775280898876405, tol)); + REQUIRE(check_almost_equal(filt2.cy_n_2, 0.820224719101123, tol)); + + } + + SECTION( "Filter coefficient computation from poles" ) + { + Real a = 1.5, + dt = 0.1; + Complex p1(-1,2), p2(-1,-2); + FlyByWire::Filter2 filt2 = FlyByWire::Filter2::FromPoles(a, p1, p2, dt); + REQUIRE(check_almost_equal(filt2.cx_n, 0.016853932584270, tol)); + REQUIRE(check_almost_equal(filt2.cx_n_1, 0.033707865168539, tol)); + REQUIRE(check_almost_equal(filt2.cx_n_2, 0.016853932584270, tol)); + REQUIRE(check_almost_equal(filt2.cy_n_1, -1.775280898876405, tol)); + REQUIRE(check_almost_equal(filt2.cy_n_2, 0.820224719101123, tol)); + + } +}