/*
 * =====================================================================================
 *
 *       Filename:  state.cpp
 *
 *    Description:  A Class for managing body and features.
 *
 *        Version:  1.0
 *        Created:  03/17/2017 07:55:56 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Martin Miller (MHM), miller7@illinois.edu
 *   Organization:  Aerospace Robotics and Controls Lab (ARC)
 *
 * =====================================================================================
 */
#include "state.h"

void 
State::update (Matrix<double,9,1> &X, Matrix<double,9,9> &P, 
        const Matrix<double,1,9> H, const Matrix<double,1,1> &h,
        const Matrix<double,1,1> &z, const Matrix<double,1,1> &R)
{
    Matrix<double,1,1> S;
    Matrix<double,9,1> K;
    S = H*P*H.transpose() + R;
    K = P*H.transpose()*S.inverse();
    P = (Matrix<double,9,9>::Identity()-K*H)*P;
    P = 0.5*(P+P.transpose());
    X += K*(z-h);
}

/*
 *--------------------------------------------------------------------------------------
 *       Class:  State
 *      Method:  State :: Pkk1
 * Description:  Updates P_k|k-1
 *--------------------------------------------------------------------------------------
 */
    void
State::Pkk1 ( Matrix<double,9,9> &P, const Matrix<double,9,9> &F,
        const Matrix<double,9,9> &Q )
{
    P = F*P*F.transpose()+Q;
    // Enforce symmetry
    P = 0.5*(P+P.transpose());
    return ;
}		/* -----  end of method State::Pkk1  ----- */