KalmanFilter
boost.h
1 #ifndef VECTORBOOST
2 #define VECTORBOOST
3 
4 #include <boost/numeric/ublas/vector.hpp>
5 #include <boost/numeric/ublas/matrix.hpp>
6 #include <boost/qvm/mat_operations.hpp>
7 #include <boost/numeric/ublas/vector_proxy.hpp>
8 #include <boost/numeric/ublas/triangular.hpp>
9 #include <boost/numeric/ublas/lu.hpp>
10 #include <boost/numeric/ublas/io.hpp>
11 #include <lapacke.h>
12 
13 #include <random>
14 
15 #include <iostream>
16 
17 
19  private:
20 
21  boost::numeric::ublas::matrix<double> value;
22  boost::numeric::ublas::range r1;
23  boost::numeric::ublas::range r2;
24  boost::numeric::ublas::matrix<double>valBack;
25  bool subMatrixCopy = false;
26 
27  //http://www.crystalclearsoftware.com/cgi-bin/boost_wiki/wiki.pl?LU_Matrix_Inversion
28  bool InvertMatrix(const boost::numeric::ublas::matrix<double>& input, boost::numeric::ublas::matrix<double>& inverse) {
29  using namespace boost::numeric::ublas;
30  typedef permutation_matrix<std::size_t> pmatrix;
31  // create a working copy of the input
32  matrix<double> A(input);
33  // create a permutation matrix for the LU-factorization
34  pmatrix pm(A.size1());
35  // perform LU-factorization
36  int res = lu_factorize(A,pm);
37  if( res != 0 ) return false;
38  // create identity matrix of "inverse"
39  inverse.assign(boost::numeric::ublas::identity_matrix<double>(A.size1()));
40  // backsubstitute to get the inverse
41  lu_substitute(A, pm, inverse);
42  return true;
43  }
44 
45  void setSystemValue(boost::numeric::ublas::matrix<double> v){
46  value = v;
47  }
48 
49  public:
50  matrixBoost(){}
51 
52  matrixBoost(int rowCount, int colCount){
53  value = boost::numeric::ublas::zero_matrix<double>(rowCount,colCount);
54  }
55 
56  matrixBoost(boost::numeric::ublas::matrix<double> value):value(value){}
57 
58  static matrixBoost Identity(int dimension,int dimension2){
59  return matrixBoost(boost::numeric::ublas::identity_matrix<double>(dimension,dimension2));
60  }
61 
62  matrixBoost transpose(){
63  matrixBoost t(boost::numeric::ublas::trans(value));
64  return t;
65  }
66 
67  matrixBoost inverse(){
68  boost::numeric::ublas::matrix<double> inverse(value.size1(),value.size2());
69  InvertMatrix(value,inverse);
70  matrixBoost t(inverse);
71  return t;
72  }
73 
74  boost::numeric::ublas::matrix<double> getSystemValue() const{
75  return value;
76  }
77 
78  matrixBoost & block(int i1, int i2, int l1, int l2){
79  if(subMatrixCopy){
80  std::swap(valBack,value);
81  }
82  subMatrixCopy = true;
83  r1 = boost::numeric::ublas::range(i1,i1+l1);
84  r2 = boost::numeric::ublas::range(i2,i2+l2);
85  valBack = boost::numeric::ublas::project(value,r1,r2);
86  std::swap(valBack,value);
87  return *this;
88  }
89 
90  matrixBoost &operator=( const matrixBoost other){
91  if(subMatrixCopy){
92  subMatrixCopy = false;
93  boost::numeric::ublas::project(valBack,r1,r2) = other.getSystemValue();
94  std::swap(valBack,value);
95  }else{
96  value = other.getSystemValue();
97  }
98  return *this;//better be no chaining of equal signs
99  }
100 
101  static void lowerTriangulate(matrixBoost & val){
102  boost::numeric::ublas::matrix<double> in= val.getSystemValue();
103  int rows = in.size1();
104  int cols = in.size2();
105  for(int i = 0; i < rows; i++){
106  int length = cols - i;
107  boost::numeric::ublas::vector<double> v = (row(in,i));
108  v = boost::numeric::ublas::subrange(v,i,cols);
109  double mew = boost::numeric::ublas::norm_2(v);
110  if(mew!=0){
111  double beta = v(0) + (-2*(v(0) < 0)+1)*mew;
112  v = v/beta;
113  }
114  v(0) = 1;
115  boost::numeric::ublas::range t1(i,rows);
116  boost::numeric::ublas::range t2(i,cols);
117  boost::numeric::ublas::vector<double> w = -2.0/
118  (boost::numeric::ublas::norm_2(v)*boost::numeric::ublas::norm_2(v))*
119  (prod(project(in,t1,t2),v));
120 
121  boost::numeric::ublas::matrix<double> vM(v.size(),1);
122  for(int i = 0; i < v.size(); i++){
123  vM(i,0) = v(i);
124  }
125  boost::numeric::ublas::matrix<double> wM(w.size(),1);
126  for(int i = 0; i < w.size(); i++){
127  wM(i,0) = w(i);
128  }
129  project(in,t1,t2) = project(in,t1,t2) + prod(wM,boost::numeric::ublas::trans(vM));
130  }
131  val.setSystemValue(in);
132  }
133 };
134 
136  private:
137  boost::numeric::ublas::vector<double> value;
138  public:
139  vectorBoost(){
140  }
141 
142  vectorBoost(boost::numeric::ublas::vector<double> value):value(value){}
143 
144  boost::numeric::ublas::vector<double> getSystemValue() const{
145  return value;
146  }
147 
148  static vectorBoost randomVector(int dimension,uint32_t seed){
149  std::mt19937 generator(seed);
150  std::normal_distribution<double> distribution(0.0,1.0);
151  boost::numeric::ublas::vector<double> tmp(dimension);
152  for(int i = 0; i < dimension; i++){
153  tmp(i) = distribution(generator);
154  }
155  return vectorBoost(tmp);
156  }
157 
158  static vectorBoost solve(matrixBoost A, vectorBoost b){
159  boost::numeric::ublas::matrix<double> Alocal = A.getSystemValue();
160  boost::numeric::ublas::vector<double> blocal = b.getSystemValue();
161  boost::numeric::ublas::permutation_matrix<size_t> pm(Alocal.size1());
162 
163  boost::numeric::ublas::lu_factorize(Alocal,pm);
164  boost::numeric::ublas::lu_substitute(Alocal, pm, blocal);
165  return vectorBoost(blocal);
166  }
167 };
168 
169 
170 vectorBoost operator-(const vectorBoost &lhs,const vectorBoost &rhs){
171  vectorBoost res(lhs.getSystemValue() - rhs.getSystemValue());
172  return res;
173 };
174 
175 vectorBoost operator+(const vectorBoost &lhs,const vectorBoost &rhs){
176  vectorBoost res(lhs.getSystemValue() + rhs.getSystemValue());
177  return res;
178 };
179 
180 std::ostream& operator<<(std::ostream& stream, const vectorBoost & vec) {
181  boost::numeric::ublas::vector<double> v = vec.getSystemValue();
182  for(int i = 0; i < v.size()-1; i++){
183  stream<<double(v(i))<<'\n';
184  }
185  stream<<v(v.size()-1);
186  return stream;
187 }
188 
189 
190 
191 
192 matrixBoost operator*(const matrixBoost &lhs, const matrixBoost &rhs){
193  matrixBoost res(boost::numeric::ublas::prod(lhs.getSystemValue(),rhs.getSystemValue()));
194  return res;
195 }
196 
197 matrixBoost operator+(const matrixBoost &lhs,const matrixBoost &rhs){
198  matrixBoost res(lhs.getSystemValue() + rhs.getSystemValue());
199  return res;
200 };
201 
202 matrixBoost operator-(const matrixBoost &lhs,const matrixBoost &rhs){
203  matrixBoost res(lhs.getSystemValue() - rhs.getSystemValue());
204  return res;
205 };
206 
207 std::ostream& operator<<(std::ostream& stream, const matrixBoost & mat) {
208  boost::numeric::ublas::matrix<double> m = mat.getSystemValue();
209  for(int i = 0; i < m.size1(); i++){
210  for(int j = 0; j < m.size2(); j++){
211  stream<<m(i,j)<<" ";
212  }stream<<'\n';
213  }
214  return stream;
215 }
216 
217 
218 
219 
220 
221 
222 vectorBoost operator*(const matrixBoost &lhs, const vectorBoost &rhs){
223  vectorBoost res(boost::numeric::ublas::prod(lhs.getSystemValue(),rhs.getSystemValue()));
224  return res;
225 }
226 
227 vectorBoost operator*(const vectorBoost &lhs, const matrixBoost &rhs){
228  vectorBoost res(boost::numeric::ublas::prod(lhs.getSystemValue(),rhs.getSystemValue()));
229  return res;
230 }
231 #endif
Definition: boost.h:18
Definition: boost.h:135