Add Filter class.

This is not tested to be working yet.

1 files changed, 85 insertions, 0 deletions

diff --git a/src/filter.cpp b/src/filter.cpp
new file mode 100644
index 0000000..3ef4c58
--- /dev/null
+++ b/src/filter.cpp
@@ -0,0 +1,85 @@
+/*
+ * =====================================================================================
+ *
+ *       Filename:  filter.cpp
+ *
+ *    Description:  Class for FIR and IIR filters
+ *
+ *        Version:  1.0
+ *        Created:  03/31/2017 02:40:02 PM
+ *       Revision:  none
+ *       Compiler:  gcc
+ *
+ *         Author:  Martin Miller (MHM), miller7@illinois.edu
+ *   Organization:  Aerospace Robotics and Controls Lab (ARC)
+ *
+ * =====================================================================================
+ */
+#include "filter.h"
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Filter
+ *      Method:  Filter
+ * Description:  constructor
+ *--------------------------------------------------------------------------------------
+ */
+Filter::Filter (const VectorXd &b, const VectorXd &a)
+{
+    int rows = b.rows()+a.rows();
+    F = Matrix<double,Dynamic,1>::Zero(rows,1);
+    F.head(b.rows()) = b;
+    F.segment(b.rows(),a.rows()) = -a;
+
+    Vector3d z;
+    z << 0,0,0;
+    for (int i=0; i<b.rows(); ++i) {
+        X.push_back(z);
+    }
+    for (int i=0; i<a.rows(); ++i) {
+        Y.push_back(z);
+    }
+}  /* -----  end of method Filter::Filter  (constructor)  ----- */
+
+Vector3d
+Filter::update ( const Vector3d &x )
+{
+    X.push_front(x);
+    X.pop_back();
+
+    MatrixXd M;
+    M = listToMatrix();
+
+    Vector3d y;
+    y = F.transpose()*M;
+
+    Y.push_front(y);
+    Y.pop_back();
+
+    return y;
+}		/* -----  end of method Filter::update  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Filter
+ *      Method:  Filter :: listToMatrix
+ * Description:  Returns X and Y as a matrix
+ *--------------------------------------------------------------------------------------
+ */
+MatrixXd
+Filter::listToMatrix() {
+    Matrix<double,Dynamic,3> M;
+    int rows = X.size() + Y.size();
+    M = Matrix<double,Dynamic,3>::Zero(rows,3);
+
+    int row = 0;
+    for (auto i=X.begin(); i!=X.end(); ++i,++row) {
+        M.row(row) = *i;
+    }
+    for (auto i=Y.begin(); i!=Y.end(); ++i,++row) {
+        M.row(row) = *i;
+    }
+
+    return M;
+}
+