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.

{
  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>();
}