void StateEstimatorKinematic::dare(const Eigen::Matrix<double,6,6> &A, const Eigen::Matrix<double,6,6> &B, Eigen::Matrix<double,6,6> &P,int zDim) { Eigen::Matrix<double,6,6> Ainv = A.inverse(); Eigen::Matrix<double,6,6> ABRB; if (zDim == 6) { ABRB = Ainv * B * _R.llt().solve(B.transpose()); } else { ABRB = Ainv * B.topLeftCorner(6,zDim) * _R.topLeftCorner(zDim,zDim).llt().solve(B.topLeftCorner(6,zDim).transpose()); } Eigen::Matrix<double,2*6,2*6> Z; Z.block(0,0,6,6) = Ainv; Z.block(0,6,6,6) = ABRB; Z.block(6,0,6,6) = _Q * Ainv; Z.block(6,6,6,6) = A.transpose() + _Q * ABRB; Eigen::ComplexEigenSolver <Eigen::Matrix<double,2*6,2*6> > ces; ces.compute(Z); Eigen::Matrix<std::complex<double>,2*6,1> eigVal = ces.eigenvalues(); Eigen::Matrix<std::complex<double>,2*6,2*6> eigVec = ces.eigenvectors(); Eigen::Matrix<std::complex<double>,2*6,6> unstableEigVec; int ctr = 0; for (int i = 0; i < 2*6; i++) { if (eigVal(i).real()*eigVal(i).real() + eigVal(i).imag()*eigVal(i).imag() > 1) { unstableEigVec.col(ctr) = eigVec.col(i); ctr++; if (ctr > 6) break; } } Eigen::Matrix<std::complex<double>,6,6> U21inv = unstableEigVec.block(0,0,6,6).inverse(); Eigen::Matrix<std::complex<double>,6,6> PP = unstableEigVec.block(6,0,6,6) * U21inv; for (int i = 0; i < 6; i++) { for (int j = 0; j < 6; j++) { P(i,j) = PP(i,j).real(); } } }
void dare(const Eigen::Matrix<double,xDim,xDim> &A, const Eigen::Matrix<double,xDim,uDim> &B, Eigen::Matrix<double,xDim,xDim> &P) { Eigen::Matrix<double,xDim,xDim> Ainv = A.inverse(); Eigen::Matrix<double,xDim,xDim> ABRB = Ainv * B * _R.llt().solve(B.transpose()); Eigen::Matrix<double,2*xDim,2*xDim> Z; Z.block(0,0,xDim,xDim) = Ainv; Z.block(0,xDim,xDim,xDim) = ABRB; Z.block(xDim,0,xDim,xDim) = _Q * Ainv; Z.block(xDim,xDim,xDim,xDim) = A.transpose() + _Q * ABRB; Eigen::ComplexEigenSolver <Eigen::Matrix<double,2*xDim,2*xDim> > ces; ces.compute(Z); Eigen::Matrix<std::complex<double>,2*xDim,1> eigVal = ces.eigenvalues(); Eigen::Matrix<std::complex<double>,2*xDim,2*xDim> eigVec = ces.eigenvectors(); Eigen::Matrix<std::complex<double>,2*xDim,xDim> unstableEigVec; int ctr = 0; for (int i = 0; i < 2*xDim; i++) { if (eigVal(i).real()*eigVal(i).real() + eigVal(i).imag()*eigVal(i).imag() > 1) { unstableEigVec.col(ctr) = eigVec.col(i); ctr++; if (ctr > xDim) break; } } Eigen::Matrix<std::complex<double>,xDim,xDim> U21inv = unstableEigVec.block(0,0,xDim,xDim).inverse(); Eigen::Matrix<std::complex<double>,xDim,xDim> PP = unstableEigVec.block(xDim,0,xDim,xDim) * U21inv; for (int i = 0; i < xDim; i++) { for (int j = 0; j < xDim; j++) { P(i,j) = PP(i,j).real(); } } }