search

jsimpso81 / WPIlibMathLabVIEW

WPILib LabVIEW Math Library
Other
16 stars 3 forks source link

Look into WPILIB issues for LQR and DARE #22

Open jsimpso81 opened 2 years ago

jsimpso81 commented 2 years ago

Look into WPILIB issues 3489 3404 3368 3694 and maybe others...

jsimpso81 commented 2 years ago

Put notes in routines for 3694 to check if detectable.

Put note to check numerical integration routines. Some of that has already been done. 3489

Dare solver stuff 3404 -- enhancements. LQR has new constructor that adds N parameter

The guts appears to be:

Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
 const Eigen::Ref<const Eigen::MatrixXd>& A,
 const Eigen::Ref<const Eigen::MatrixXd>& B,
 const Eigen::Ref<const Eigen::MatrixXd>& Q,
 const Eigen::Ref<const Eigen::MatrixXd>& R,
 const Eigen::Ref<const Eigen::MatrixXd>& N) {
 DRAKE_DEMAND(N.rows() == B.rows() && N.cols() == B.cols());

 Eigen::MatrixXd scrA = A - B * R.llt().solve(N.transpose());
 Eigen::MatrixXd scrQ = Q - N * R.llt().solve(N.transpose());
 return DiscreteAlgebraicRiccatiEquation(scrA, B, scrQ, R);

}

jsimpso81 commented 2 years ago

This has been added. There is now a DareN function.