void LinearizedKF::Predict(const ompl::base::State *belief,
const ompl::control::Control* control, const LinearSystem& ls, ompl::base::State *predictedState)
{
using namespace arma;
this->motionModel_->Evolve(belief, control,this->motionModel_->getZeroNoise(), predictedState);
mat covPred = ls.getA() * belief->as<StateType>()->getCovariance() * trans(ls.getA()) +
ls.getG() * ls.getQ() * trans(ls.getG());
predictedState->as<StateType>()->setCovariance(covPred);
}
arma::mat LinearizedKF::computeStationaryCovariance (const LinearSystem& ls)
{
using namespace arma;
mat H = ls.getH();
mat M = ls.getM();
mat R = ls.getR();
mat Pprd;
bool dareSolvable = dare (trans(ls.getA()),trans(ls.getH()),ls.getG() * ls.getQ() * trans(ls.getG()),
ls.getM() * ls.getR() * trans(ls.getM()), Pprd );
if(!dareSolvable)
{
OMPL_ERROR("Dare Unsolvable: The given system state is not stable/observable. Try a different state.");
exit(1);
}
//makes this symmetric
Pprd = (Pprd + trans(Pprd)) / 2;
mat Pest = Pprd - ( Pprd * H.t()) * inv( H*Pprd*H.t() + M * R * M.t(), true) * trans(Pprd * H.t()) ;
//makes this symmetric
Pest = (Pest + trans(Pest)) / 2;
return Pest;
}