Add Filter class.

This is not tested to be working yet.

3 files changed, 137 insertions, 2 deletions

diff --git a/Makefile b/Makefile
index 4710ab9..af2e0ee 100644
--- a/Makefile
+++ b/Makefile
@@ -1,7 +1,7 @@
-OBJECT=src/main.o src/body.o src/state.o src/feature.o src/camera.o src/ourerr.o 
+OBJECT=src/main.o src/body.o src/state.o src/feature.o src/camera.o src/ourerr.o src/filter.o
 SRCS=$(patsubst %.o,%.cpp,$(OBJECT))
 CXXFLAGS+=-O3 -march=native -std=c++11
-#CXXFLAGS+=-g
+CXXFLAGS+=-g
 CXXFLAGS+=$(shell pkg-config --cflags eigen3 yaml-cpp)
 #CXXFLAGS+=$(shell pkg-config --cflags opencv)
 LIBS+=$(shell pkg-config --libs eigen3 yaml-cpp)
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;
+}
+
diff --git a/src/filter.h b/src/filter.h
new file mode 100644
index 0000000..fdd7777
--- /dev/null
+++ b/src/filter.h
@@ -0,0 +1,50 @@
+#ifndef  filter_INC
+#define  filter_INC
+
+#include <Eigen/Dense>
+#include <iostream>
+#include <list>
+
+using Eigen::Dynamic;
+using Eigen::Matrix;
+using Eigen::MatrixXd;
+using Eigen::VectorXd;
+using Eigen::Vector3d;
+using std::list;
+
+/*
+ * =====================================================================================
+ *        Class:  Filter
+ *  Description:  
+ * =====================================================================================
+ */
+class Filter
+{
+    public:
+        /* ====================  LIFECYCLE     ======================================= */
+        Filter (const VectorXd &b, const VectorXd &a);                             /* constructor */
+
+        /* ====================  ACCESSORS     ======================================= */
+        MatrixXd listToMatrix();
+
+        /* ====================  MUTATORS      ======================================= */
+
+        /* ====================  OPERATORS     ======================================= */
+        Vector3d update( const Vector3d &x);
+
+    protected:
+        /* ====================  METHODS       ======================================= */
+
+        /* ====================  DATA MEMBERS  ======================================= */
+
+    private:
+        /* ====================  METHODS       ======================================= */
+
+        /* ====================  DATA MEMBERS  ======================================= */
+        Matrix<double, Dynamic, 3> y;
+        Matrix<double,Dynamic,1> F;
+        list<Vector3d> X,Y;
+
+}; /* -----  end of class Filter  ----- */
+
+#endif   /* ----- #ifndef filter_INC  ----- */