void LinearizedKF::Update(const ompl::base::State *belief, const typename ObservationModelMethod::ObservationType& obs,
const LinearSystem& ls, ompl::base::State *updatedState)
{
using namespace arma;
colvec innov = this->observationModel_->computeInnovation(belief, obs);
if(!innov.n_rows || !innov.n_cols)
{
updatedState = si_->cloneState(belief);
return; // return the prediction if you don't have any innovation
}
assert(innov.n_rows);
assert(innov.n_cols);
mat covPred = belief->as<StateType>()->getCovariance();
mat rightMatrix = ls.getH() * covPred * trans(ls.getH()) + ls.getR();
mat leftMatrix = covPred * trans(ls.getH());
mat KalmanGain = solve(trans(rightMatrix), trans(leftMatrix));
KalmanGain = KalmanGain.t();
colvec xPredVec = belief->as<StateType>()->getArmaData();
colvec xEstVec = xPredVec + KalmanGain*innov;
updatedState->as<StateType>()->setXYYaw(xEstVec[0], xEstVec[1], xEstVec[2]);
mat covEst = covPred - KalmanGain* ls.getH() * covPred;
updatedState->as<StateType>()->setCovariance(covEst);
}
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;
}