continuous_to_discrete.h

Go to the documentation of this file.

#ifndef G2O_CONTINUOUS_TO_DISCRETE_H_
#define G2O_CONTINUOUS_TO_DISCRETE_H_
 
#include <iostream>
#include <unsupported/Eigen/MatrixFunctions>
 
// Form for fixed-size matrices
template <typename MatrixType>
void continuousToDiscrete(MatrixType& Fd, MatrixType& Qd, const MatrixType& Fc,
                          const MatrixType& Qc, double dt) {
  enum {
    NX = MatrixType::ColsAtCompileTime,
    NY = MatrixType::RowsAtCompileTime,
    NX2 = 2 * MatrixType::RowsAtCompileTime
  };
 
  typedef Eigen::Matrix<typename MatrixType::Scalar, NX2, NX2>
      DoubleSizedMatrixType;
  DoubleSizedMatrixType bigA(NX2, NX2), bigB(NX2, NX2);
 
  // Construct the "big A matrix"
  bigA.template topLeftCorner<NX, NX>() = -Fc * dt;
  bigA.template topRightCorner<NX, NX>() = Qc * dt;
  bigA.template bottomLeftCorner<NX, NX>().setZero();
  bigA.template bottomRightCorner<NX, NX>() = Fc.transpose() * dt;
 
  // bigB = expm(bigA)
  // Eigen::MatrixExponential<DoubleSizedMatrixType> me(bigA);
  // me.compute(bigB);
  bigB = bigA.exp();
 
  // Extract the discrete time components
  Fd = bigB.template bottomRightCorner<NX, NX>().transpose();
  Qd = Fd * bigB.template topRightCorner<NX, NX>();
}
 
#endif  // __CONTINUOUS_TO_DISCRETE_H__