1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
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;
}
|
