コード例 #1
0
    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;
    }
コード例 #2
0
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();
    }
}