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