Added Filter 1, Filter 2.

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

FlyByWire.hpp

@@ -2,8 +2,7 @@
 #define DEF_FlyByWire
 
 #include <cmath>
-
+#include <complex>
-typedef double Real;
 
 #define HUGE_VALUE_REAL Real(1e20)
 
@@ -14,6 +13,9 @@ typedef double Real;
 namespace FlyByWire
 {
 
+typedef double Real;
+typedef std::complex<Real> 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_min; //!< Min rate.
-        Real dy_dt_max; //< Max rate.
+        Real dy_dt_max; //!< Max rate.
-        Real dt;        //< Time step.
+        Real dt;        //!< Time step.
-        Real y_n_1;     //< Previous filter value.
+        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 Ts;//!< Time step.
-        Real lower_bound;//< Time step.
+        Real lower_bound;//!< Lower bound.
-        Real upper_bound;//< Time step.
+        Real upper_bound;//!< Upper bound.
-        Real x_n_1;//< x[n-1].
+        Real x_n_1;//!< x[n-1].
-        Real y_n_1;//< y[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]
 };
 
 }

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;
 }

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));
+
+    }
+}