g2o
Functions
continuous_to_discrete.h File Reference
#include <unsupported/Eigen/MatrixFunctions>
#include <iostream>
Include dependency graph for continuous_to_discrete.h:
This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Functions

template<typename MatrixType >
void continuousToDiscrete (MatrixType &Fd, MatrixType &Qd, const MatrixType &Fc, const MatrixType &Qc, double dt)
 

Function Documentation

template<typename MatrixType >
void continuousToDiscrete ( MatrixType &  Fd,
MatrixType &  Qd,
const MatrixType &  Fc,
const MatrixType &  Qc,
double  dt 
)

Definition at line 9 of file continuous_to_discrete.h.

11 {
12  enum
13  {
14  NX = MatrixType::ColsAtCompileTime,
15  NY = MatrixType::RowsAtCompileTime,
16  NX2 = 2 * MatrixType::RowsAtCompileTime
17  };
18 
19  typedef Eigen::Matrix<typename MatrixType::Scalar,NX2,NX2> DoubleSizedMatrixType;
20  DoubleSizedMatrixType bigA(NX2,NX2), bigB(NX2,NX2);
21 
22  // Construct the "big A matrix"
23  bigA.template topLeftCorner<NX,NX>()=-Fc*dt;
24  bigA.template topRightCorner<NX,NX>()= Qc * dt;
25  bigA.template bottomLeftCorner<NX,NX>().setZero();
26  bigA.template bottomRightCorner<NX,NX>()=Fc.transpose() * dt;
27 
28  // bigB = expm(bigA)
29  //Eigen::MatrixExponential<DoubleSizedMatrixType> me(bigA);
30  //me.compute(bigB);
31  bigB = bigA.exp();
32 
33  // Extract the discrete time components
34  Fd = bigB.template bottomRightCorner<NX,NX>().transpose();
35  Qd = Fd * bigB.template topRightCorner<NX,NX>();
36 }