diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3bf95088b41084f327a217704aa2706b00675720..79ed11471e96fe7b843d384a0cb466c248b66392 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -59,6 +59,7 @@ set(${PROJECT_NAME}_HEADERS
include/qrw/Params.hpp
include/qrw/Estimator.hpp
include/qrw/Joystick.hpp
+ include/qrw/Filter.hpp
include/other/st_to_cc.hpp
)
@@ -75,6 +76,7 @@ set(${PROJECT_NAME}_SOURCES
src/Params.cpp
src/Estimator.cpp
src/Joystick.cpp
+ src/Filter.cpp
)
add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
diff --git a/include/qrw/Filter.hpp b/include/qrw/Filter.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9b3b34176171ccc49913a1fb4cadcdd7ef048e73
--- /dev/null
+++ b/include/qrw/Filter.hpp
@@ -0,0 +1,75 @@
+#ifndef FILTER_H_INCLUDED
+#define FILTER_H_INCLUDED
+
+#include <iostream>
+#include <fstream>
+#include <cmath>
+#include <deque>
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include "qrw/Params.hpp"
+#include "pinocchio/math/rpy.hpp"
+
+class Filter
+{
+public:
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Constructor
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ Filter();
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Initialize with given data
+ ///
+ /// \param[in] params Object that stores parameters
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void initialize(Params& params);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Destructor.
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ~Filter() {} // Empty destructor
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Run one iteration of the filter
+ ///
+ /// \param[in] x Quantity to filter
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void filter(VectorN const& x);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Add or remove 2 PI to all elements in the queues to handle +- pi modulo
+ ///
+ /// \param[in] a Angle that needs change (3, 4, 5 for Roll, Pitch, Yaw respectively)
+ /// \param[in] dir Direction of the change (+pi to -pi or -pi to +pi)
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void handle_modulo(int a, bool dir);
+
+ VectorN getFilt() { return y_; }
+
+private:
+
+ Vector5 b_; // Denominator coefficients of the filter transfer function
+ Vector5 a_; // Numerator coefficients of the filter transfer function
+ Vector6 x_; // Latest measurement
+ VectorN y_; // Latest result
+ Vector6 accum_; // Used to compute the result (accumulation)
+
+ std::deque<Vector6> x_queue_; // Store the last measurements for product with denominator coefficients
+ std::deque<Vector6> y_queue_; // Store the last results for product with numerator coefficients
+
+ bool init_; // Initialisation flag
+
+};
+
+#endif // FILTER_H_INCLUDED
diff --git a/src/Filter.cpp b/src/Filter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..291cfc5e91d92cf83101b2c7a0a47e34f40f2648
--- /dev/null
+++ b/src/Filter.cpp
@@ -0,0 +1,104 @@
+#include "qrw/Filter.hpp"
+
+Filter::Filter()
+ : b_(Vector5::Zero())
+ , a_(Vector5::Zero())
+ , x_(Vector6::Zero())
+ , y_(VectorN::Zero(6, 1))
+ , accum_(Vector6::Zero())
+ , init_(false)
+{
+ // Empty
+}
+
+void Filter::initialize(Params& params)
+{
+ if (params.dt_wbc == 0.001)
+ {
+ b_ << 3.12389769e-5, 1.24955908e-4, 1.87433862e-4, 1.24955908e-4, 3.12389769e-5;
+ a_ << 1., -3.58973389, 4.85127588, -2.92405266, 0.66301048;
+ }
+ else if (params.dt_wbc == 0.002)
+ {
+ b_ << 0.0004166, 0.0016664, 0.0024996, 0.0016664, 0.0004166;
+ a_ << 1., -3.18063855, 3.86119435, -2.11215536, 0.43826514;
+ }
+ else
+ {
+ throw std::runtime_error("No coefficients defined for this time step.");
+ }
+
+ x_queue_.resize(b_.rows(), Vector6::Zero());
+ y_queue_.resize(a_.rows()-1, Vector6::Zero());
+}
+
+void Filter::filter(VectorN const& x)
+{
+ // If x is position + quaternion then we convert quaternion to RPY
+ if (x.rows() == 7)
+ {
+ x_.head(3) = x.head(3);
+ Eigen::Quaterniond quat(x(6, 0), x(3, 0), x(4, 0), x(5, 0)); // w, x, y, z
+ x_.tail(3) = pinocchio::rpy::matrixToRpy(quat.toRotationMatrix());
+
+ // Handle 2 pi modulo for roll, pitch and yaw
+ // Should happen sometimes for yaw but now for roll and pitch except
+ // if the robot rolls over
+ for (int i = 3; i < 6; i++)
+ {
+ if (std::abs(x_(i, 0) - y_(i, 0)) > 1.5 * M_PI)
+ {
+ std::cout << "Modulo for " << i << std::endl;
+ handle_modulo(i, x_(i, 0) - y_(i, 0) > 0);
+ }
+ }
+ }
+ else // Otherwise we can directly use x
+ {
+ x_ = x;
+ }
+
+ // Initialisation of the value in the queues to the first measurement
+ if (!init_)
+ {
+ init_ = true;
+ std::fill(x_queue_.begin(), x_queue_.end(), x_.head(6));
+ std::fill(y_queue_.begin(), y_queue_.end(), x_.head(6));
+ }
+
+ // Store measurement in x queue
+ x_queue_.pop_back();
+ x_queue_.push_front(x_.head(6));
+
+ // Compute result (y/x = b/a for the transfert function)
+ accum_ = Vector6::Zero();
+ for (int i = 0; i < b_.rows(); i++)
+ {
+ accum_ += b_[i] * x_queue_[i];
+ }
+ for (int i = 1; i < a_.rows(); i++)
+ {
+ accum_ -= a_[i] * y_queue_[i-1];
+ }
+
+ // Store result in y queue for recursion
+ y_queue_.pop_back();
+ y_queue_.push_front(accum_ / a_[0]);
+
+ // Filtered result is stored in y_queue_.front()
+ // Assigned to dynamic-sized vector for binding purpose
+ y_ = y_queue_.front();
+}
+
+void Filter::handle_modulo(int a, bool dir)
+{
+ // Add or remove 2 PI to all elements in the queues
+ for (int i = 0; i < b_.rows(); i++)
+ {
+ (x_queue_[i])(a, 0) += dir ? 2.0 * M_PI : -2.0 * M_PI;
+ }
+ for (int i = 1; i < a_.rows(); i++)
+ {
+ (y_queue_[i-1])(a, 0) += dir ? 2.0 * M_PI : -2.0 * M_PI;
+ }
+}