KalmanFilter
eigenSimple.h
1 #ifndef EIGENSIMPLE
2 #define EIGENSIMPLE
3 
4 EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE
5 Derived randomVector(int dimension, uint32_t seed){
6  std::mt19937 generator(seed);
7  std::normal_distribution<double> distribution(0.0,1.0);
8  Derived res(dimension);
9  for(int i = 0; i < dimension; i++){
10  res[i] = distribution(generator);
11  }
12  return res;
13 }
14 
15 
16 void static lowerTriangulate(Eigen::MatrixBase<Derived> &in){
17  for(int i = 0; i < in.rows(); i++){
18  int length = in.cols() - i;
19  Derived v = in.row(i).tail(length).transpose();
20  double mew = v.norm();
21  if(mew!=0){
22  double beta = v(0) + (-2*(v(0) < 0)+1)*mew;
23  v = v/beta;
24  }
25  v(0) = 1;
26  Derived w = -2.0/(v.squaredNorm())*(in.block(i,i, in.rows()-i , in.cols()-i)*v);
27  in.block(i,i, in.rows()-i , in.cols()-i) = in.block(i,i, in.rows()-i , in.cols()-i) + w*v.transpose();
28  }
29 }
30 
31 //Eigen::Matrix<double, -1, 1> A,Eigen::Matrix<double, -1, -1> b){
32 static Derived solve(Derived A, Derived b){
33  return A.colPivHouseholderQr().solve(b);
34 }
35 
36 
37 
38 
39 
40 
41 #endif