diff options
| -rw-r--r-- | Makefile | 4 | ||||
| -rw-r--r-- | src/filter.cpp | 85 | ||||
| -rw-r--r-- | src/filter.h | 50 |
3 files changed, 137 insertions, 2 deletions
@@ -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 ----- */ |