void infTimeLQR(const Eigen::Matrix<double,xDim,xDim> & A,
const Eigen::Matrix<double,xDim,uDim> & B)
{
reset();
/*
for (int i=0; i<infIter; i++) {
#if 0
std::cout << "K: " << std::endl << _K;
<< "V: " << std::endl << _V;
<< "===========================================" << std::endl;
#endif
LQRBackup(A, B);
}
std::cout << "loop K\n" << _K << std::endl;
std::cout << "loop V\n" << _V << std::endl;
*/
dare(A, B, _V);
_K = -(B.transpose() * _V * B + _R).llt().solve(B.transpose() * _V * A);
//std::cout << "dare K\n" << _K << std::endl;
//std::cout << "dare V\n" << _V << std::endl;
}
void StateEstimatorKinematic::computeKss(const Eigen::Matrix<double,6,6> &A, const Eigen::Matrix<double,6,6> &C, int zDim )
{
Eigen::Matrix<double,6,6> B = C.transpose();
// Eigen::Matrix<double,6,6> P;
dare(A.transpose(), B, _P, zDim); // A^T is used here
_K.setZero();
Eigen::Matrix<double,6,6> PB;
Eigen::Matrix<double,6,6> BtPB_R;
if (zDim == 6) {
PB = _P * B;
BtPB_R = B.transpose() * PB + _R;
_K = PB* BtPB_R.inverse()
;
}
else
{
PB.topLeftCorner(6,zDim) = _P * B.topLeftCorner(6,zDim);
BtPB_R.topLeftCorner(zDim,zDim) = B.topLeftCorner(6,zDim).transpose() * PB.topLeftCorner(6,zDim) + _R.topLeftCorner(zDim,zDim);
_K.topLeftCorner(6,zDim) = PB.topLeftCorner(6,zDim)* BtPB_R.topLeftCorner(zDim,zDim).inverse();
}
}
