diff --git a/FlyByWire.cpp b/FlyByWire.cpp
index 3cd3c80..9ecb44e 100644
--- a/FlyByWire.cpp
+++ b/FlyByWire.cpp
@@ -130,6 +130,12 @@ Real Integrator1::Filter(Real x_n)
     return y_n;
 }
 
+Filter1::Filter1()
+{
+    SetState(Real(0.));
+    ComputeCoeffsFromContinuousTF(Real(0.), Real(1.), Real(0.), Real(1.), Real(1.));
+}
+
 Filter1::Filter1(Real b1, Real b0, Real a1, Real a0, Real Ts, Real y0)
 {
     SetState(y0);
@@ -233,4 +239,67 @@ Filter2 Filter2::Bandstop(Real omega, Real Q, Real Ts)
     return Filter2(Real(1.), Real(0.), omegasqr, Real(1.), omega/Q, omegasqr, Ts);
 }
 
+// =============================================================================
+// PID controller
+// =============================================================================
+
+PID::PID(Real Kp_, Real Ki_, Real Kd_, Real Ts_, Real output_lower_bound_, Real output_upper_bound_, Real tau_filtered_derivative, Real integrator_lower_bound_, Real integrator_upper_bound_, bool auto_anti_windup_, Real auto_anti_windup_margin_)
+    :   Kp(Kp_),
+        Ki(Ki_),
+        Kd(Kd_),
+        Ts(Ts_),
+        output_lower_bound(output_lower_bound_),
+        output_upper_bound(output_upper_bound_),
+        auto_anti_windup_margin(auto_anti_windup_margin_),
+        u_n_1(Real(0.)),
+        auto_anti_windup(auto_anti_windup_),
+        freeze_integration(false)
+{
+    integrator = Integrator1(Ts, integrator_lower_bound_, integrator_upper_bound_);
+    differentiator = Filter1::FilteredDerivative(Ts, tau_filtered_derivative);
+}
+
+Real PID::Filter(Real e_n)
+{
+    Real inte = integrator.Filter(Real(!ComputeFreezeIntegrator())*e_n);
+    Real dere = differentiator.Filter(e_n);
+    Real u = Kp*e_n + Ki*inte + Kd*dere;
+    u = Sat1(u, output_lower_bound, output_upper_bound);
+    u_n_1 = u;
+    return u;
+}
+
+void PID::SetFreezeIntegration(bool freeze_integration_)
+{
+    freeze_integration = freeze_integration_;
+}
+
+void PID::SetFilteredDerivativeTimeConstant(Real tau_filtered_derivative)
+{
+    Filter1 differentiator2 = Filter1::FilteredDerivative(Ts, tau_filtered_derivative);// Recompute filter coefficients
+    differentiator2.x_n_1 = differentiator.x_n_1;
+    differentiator2.y_n_1 = differentiator.y_n_1;// copy current state of filter for continuity
+    differentiator = differentiator2;
+}
+
+void PID::SetIntegratorLimits(Real integrator_lower_bound, Real integrator_upper_bound)
+{
+    integrator.lower_bound = integrator_lower_bound;
+    integrator.upper_bound = integrator_upper_bound;
+}
+
+void PID::SetIntegratorValue(Real integrator_value)
+{
+    integrator.SetState(integrator_value, integrator_value);
+}
+
+bool PID::ComputeFreezeIntegrator()
+{
+    // automatic anti-windup freezes the integrator when the option is activated and the input is past either bound minus the margin
+    bool anti_windup_active = auto_anti_windup && ((u_n_1 <= output_lower_bound + auto_anti_windup_margin) || (u_n_1 >= output_upper_bound - auto_anti_windup_margin));
+
+    // Final order is computed from the user-forced inhibition and the automatic inhibition
+    return anti_windup_active || freeze_integration;
+}
+
 }// namespace FlyByWire
diff --git a/FlyByWire.hpp b/FlyByWire.hpp
index 905dbeb..8b80e1c 100644
--- a/FlyByWire.hpp
+++ b/FlyByWire.hpp
@@ -10,6 +10,13 @@
 // General fly-by-wire helper functions
 // =============================================================================
 
+#ifndef MIN
+	#define MIN(a,b) (((a) < (b)) ? (a) : (b))
+#endif
+#ifndef MAX
+	#define MAX(a,b) (((a) > (b)) ? (a) : (b))
+#endif
+
 namespace FlyByWire
 {
 
@@ -35,7 +42,7 @@ Real Sat1(Real x, Real a, Real b);
 /// Limits the rate of the signal x(t) : [dx/dt](t) to the interval [dy_dt_min, dy_dt_max].
 /// Use like a recursive filter.
 ///
-/// \variable x_n             : Input value at step n
+/// \variable x_n              : Input value at step n
 /// \variable y_n_1            : Output value at step n-1
 /// \variable dy_dt_min        : Min derivative
 /// \variable dy_dt_max        : Max derivative
@@ -84,7 +91,7 @@ class Integrator1
 {
     public:
         /// Creates an integrator object. By default, y0 = 0, and the limits are +/- HUGE_VALUE_REAL.
-        Integrator1(Real Ts_, Real y_ = Real(0.), Real lower_bound_ = -HUGE_VALUE_REAL, Real upper_bound_ = HUGE_VALUE_REAL);
+        Integrator1(Real Ts_ = Real(1.), Real y_ = Real(0.), Real lower_bound_ = -HUGE_VALUE_REAL, Real upper_bound_ = HUGE_VALUE_REAL);
 
         /// Sets the state of the filter so that it can be initialized at any value.
         /// By default, the states are initialized to 0.
@@ -143,6 +150,9 @@ class Integrator1
 class Filter1
 {
     public:
+		/// Creates pass-through Filter1 object.
+		Filter1();
+
         /// 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.));
 
@@ -221,7 +231,6 @@ class Filter1
 ///     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:
@@ -308,6 +317,75 @@ class Filter2
         Real y_n_2;     //!< y[n-2]
 };
 
+// =============================================================================
+// PID controller
+// =============================================================================
+
+/// PID controller in its parallel form :
+/// u = Kp*e + Ki*int(e*dt, t) + Kd*de/dt
+///
+/// The derivative of the error is computed using a 1st order discretized filtered derivative filter.
+/// The integral of the error is computed using the trapezoïdal rule.
+/// The integrator can be limited using integrator_lower_bound and integrator_upper_bound.
+/// The integration can be frozen using the boolean freeze_integration (true to freeze it).
+/// The output can be limited using output_lower_bound and output_upper_bound.
+///
+/// An automatic anti-windup functionnality is provided : the user can choose a value a past which
+/// the integrator is automatically frozen in order to prevent wind-ups.
+/// For instance, if margin = 0.05 and the limits are [-1 ; 1], the integrator will be frozen
+/// in the intervals [-1 ; -0.95] and [0.95 ; 1]
+class PID
+{
+	public:
+		/// Initializes the PID controller with the right coefficients.
+		/// \param Kp_						: Proportionnal gain
+		/// \param Ki_						: Integral gain
+		/// \param Kd_						: Derivative gain
+		/// \param Ts_						: Time step
+		/// \param output_lower_bound_		: Lower bound of the controller output
+		/// \param output_upper_bound_		: Upper bound of the controller output
+		/// \param tau_filtered_derivative_	: Time constant of the filtered derivative
+		/// \param integrator_lower_bound_	: Lower bound of the integrator
+		/// \param integrator_upper_bound_	: Upper bound of the integrator
+		/// \param auto_anti_windup_		: State boolean to activate or deactivate the auto-anti-windup feature
+		/// \param auto_anti_windup_margin_	: Absolute margin for the auto-anti-windup feature
+		PID(Real Kp_, Real Ki_, Real Kd_, Real Ts_, Real output_lower_bound_ = -HUGE_VALUE_REAL, Real output_upper_bound_ = HUGE_VALUE_REAL, Real tau_filtered_derivative = Real(0.01), Real integrator_lower_bound_ = -HUGE_VALUE_REAL, Real integrator_upper_bound_ = HUGE_VALUE_REAL, bool auto_anti_windup_ = true, Real auto_anti_windup_margin_ = Real(1e-3));
+
+		/// Computes the next controller output value of the PID u[n] from the next error value e[n].
+        Real Filter(Real e_n);
+
+		/// Sets the state of the integration freezing feature. True to freeze the integration.
+		void SetFreezeIntegration(bool freeze_integration_);
+
+		/// Sets the time constant of the filtered derivative.
+		void SetFilteredDerivativeTimeConstant(Real tau_filtered_derivative);
+
+		/// Sets the integrator limits.
+		void SetIntegratorLimits(Real integrator_lower_bound, Real integrator_upper_bound);
+
+		/// Sets the current integrator value.
+		void SetIntegratorValue(Real integrator_value);
+
+		/// Computes whether the integrator should be frozen or not.
+		bool ComputeFreezeIntegrator();
+
+	public:
+		Real Kp;							//!< Proportionnal gain
+		Real Ki;							//!< Integral gain
+		Real Kd;							//!< Derivative gain
+		Real Ts;							//!< Time step
+		Real output_lower_bound;			//!< Lower bound of the controller output
+		Real output_upper_bound;			//!< Upper bound of the controller output
+		Real auto_anti_windup_margin;		//!< Margin for the auto-anti-windup feature
+		Real u_n_1;							//!< Previous controller output
+
+		bool auto_anti_windup;				//!< State boolean to activate the auto-anti-windup feature
+		bool freeze_integration;			//!< State boolean to freeze the integration of the error
+
+		Integrator1 integrator;				//!< Discrete 1st-order integrator
+		Filter1 differentiator;				//!< Discrete 1st-order filtered derivative
+};
+
 }
 
 #endif
diff --git a/main.cpp b/main.cpp
index 3f56158..70f2923 100644
--- a/main.cpp
+++ b/main.cpp
@@ -10,54 +10,121 @@ using std::endl;
 using FlyByWire::Real;
 using FlyByWire::Complex;
 
+void test_Filter1_step_response();
+void test_Filter2_random_noise();
+void test_PID_simple_system();
+
 int main()
 {
 	cout.precision(16);
 
-	if(0)
-	{// step response of simple 1st order filter
-		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.);
-		}
-	}
-
-	// if(0)
-	{// Generate some uniform noise and filter it to see if the filter class works as expected
-		uint32_t state[5] = {123456, 654897, 812656, 87913951, 0};
-
-		double	tend = 10.,
-				dt = 0.025,
-				omega = 2*M_PI*10,
-				Q = 20.,
-				t, y = 0., y2;
-		FlyByWire::Filter2 filt1 = FlyByWire::Filter2::Bandpass(omega, Q, dt);
-
-		std::ofstream out("data_out.m", std::ios_base::out | std::ios_base::trunc);
-
-		if(!out.is_open())
-		{
-			std::cerr << "Could not open the file in writing mode.\n";
-			return 1;
-		}
-
-		out << "a = [";
-		for(int i = 0 ; i <= (tend/dt) ; i++)
-		{
-			t = i*dt;
-			y = RAND_XORWOW_A_B(state, -1., 1.);
-			y2 = filt1.Filter(y);
-			out << t << " " << y << " " << y2 << "\n";
-		}
-		out << "];\nplot(a(:,1), a(:,2), a(:,1), a(:,3), 'linewidth', 2.), grid on, legend('original', 'filtered')";
-	}
+	// test_Filter1_step_response();
+	// test_Filter2_random_noise();
+	test_PID_simple_system();
 
 	return 0;
 }
+
+void test_Filter1_step_response()
+{
+	// step response of simple 1st order filter
+	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.);
+	}
+}
+
+void test_Filter2_random_noise()
+{
+	// Generate some uniform noise and filter it to see if the filter class works as expected
+	uint32_t state[5] = {123456, 654897, 812656, 87913951, 0};
+
+	double	tend = 10.,
+			dt = 0.025,
+			omega = 2*M_PI*10,
+			Q = 20.,
+			t, y = 0., y2;
+	FlyByWire::Filter2 filt1 = FlyByWire::Filter2::Bandpass(omega, Q, dt);
+
+	std::ofstream out("data_out.m", std::ios_base::out | std::ios_base::trunc);
+
+	if(!out.is_open())
+	{
+		std::cerr << "Could not open the file in writing mode.\n";
+		return;
+	}
+
+	out << "a = [";
+	for(int i = 0 ; i <= (tend/dt) ; i++)
+	{
+		t = i*dt;
+		y = RAND_XORWOW_A_B(state, -1., 1.);
+		y2 = filt1.Filter(y);
+		out << t << " " << y << " " << y2 << "\n";
+	}
+	out << "];\nplot(a(:,1), a(:,2), a(:,1), a(:,3), 'linewidth', 2.), grid on, legend('original', 'filtered')";
+}
+
+void test_PID_simple_system()
+{
+	// Simple mass system with gravity g and perturbation w
+	// xdotdot = -g + u + w
+	const Real tf = 10., dt = 1./50.;
+	const Real g = 9.81, wMag = 1.;
+	const Real Kp = 1., Ki = 0.1, Kd = 0.1;
+	Real x = 0., xdot = 0., xdotdot = 0., u, w, t, xd, e;
+
+	uint32_t state[5] = {123456, 654897, 812656, 87913951, 0};// noise generator state
+
+	FlyByWire::PID pid(
+		Kp,
+		Ki,
+		Kd,
+		dt,
+		-HUGE_VALUE_REAL,//output_lower_bound_
+		HUGE_VALUE_REAL,//output_upper_bound_
+		Real(0.01),//tau_filtered_derivative
+		-HUGE_VALUE_REAL,//integrator_lower_bound_
+		HUGE_VALUE_REAL,//integrator_upper_bound_
+		true,// auto_anti_windup_
+		1e-3);// anti_windup margin
+
+	// Output to file
+	std::ofstream out("data_out.m", std::ios_base::out | std::ios_base::trunc);
+
+	if(!out.is_open())
+	{
+		std::cerr << "Could not open the file in writing mode.\n";
+		return;
+	}
+
+	out << "a = [";
+	for(t = 0. ; t <= tf ; t += dt)
+	{
+		// compute system target
+		xd = sin(2*M_PI*t/2);
+		e = xd-x;
+
+		// compute system Input
+		// u = pid.Filter(e);
+
+		// simulate system
+		w = RAND_XORWOW_A_B(state, -wMag, wMag);
+		xdotdot  = -g + u + w;
+		xdot 	+= xdotdot*dt;	// dirty Forward Euler
+		x 		+= xdot*dt;		// dirty Forward Euler
+
+		// write to file
+		out << t << " " << xd << " " << x << " " << xdot << " " << xdotdot << " " << e << " " << u << " " << w << "\n";
+	}
+
+	out << "];\nfigure(1), plot(a(:,1), a(:,2), a(:,1), a(:,3), 'linewidth', 2.), grid on, legend('xd', 'x')\n";
+	out << "figure(2), plot(a(:,1), a(:,6), a(:,1), a(:,7), 'linewidth', 2.), grid on, legend('e', 'u')\n";
+}
diff --git a/test/1_test.cpp b/test/1_test.cpp
index 5a87c73..376169b 100644
--- a/test/1_test.cpp
+++ b/test/1_test.cpp
@@ -31,13 +31,22 @@ TEST_CASE( "Basic FlyByWire functions", "[BasicFBW]" )
         REQUIRE(check_almost_equal(FlyByWire::Hdg_err(4.71238898038469, 0.17453292519943295), -1.7453292519943295, tol));
     }
 
+    SECTION( "MIN/MAX macros" ) {
+        Real a = 1.5, b = -1.;
+        Real c = MIN(a,b);
+        REQUIRE(c == b);
+        c = MAX(a,b);
+        REQUIRE(c == a);
+    }
+
     SECTION( "Vote" ) {
         Real a = 1.5, b = -1., c = 3.;
-        REQUIRE(FlyByWire::Vote(a,b,c) == a);
-        REQUIRE(FlyByWire::Vote(a,c,b) == a);
-        REQUIRE(FlyByWire::Vote(b,a,c) == a);
-        REQUIRE(FlyByWire::Vote(c,b,a) == a);
-        REQUIRE(FlyByWire::Vote(c,a,b) == a);
+        CHECK(FlyByWire::Vote(a,b,c) == a);
+        CHECK(FlyByWire::Vote(a,c,b) == a);
+        CHECK(FlyByWire::Vote(b,a,c) == a);
+        CHECK(FlyByWire::Vote(b,c,a) == a);
+        CHECK(FlyByWire::Vote(c,a,b) == a);
+        CHECK(FlyByWire::Vote(c,b,a) == a);
     }
 
     SECTION( "Deadzone" ) {