continuous_to_discrete.h File Reference

#include <iostream>
#include <unsupported/Eigen/MatrixFunctions>

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

continuousToDiscrete()

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