/*
 * =====================================================================================
 *
 *       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"

State::State ( )
{
    body = new Body;
    P = Matrix<double,Dynamic,Dynamic>::Zero(9,9);
    P.block(6,6,3,3) = 1e-6*Matrix3d::Identity();
    return ;
}		/* -----  end of method State::State  ----- */

void
State::vel ( const Matrix<double,3,1> &v )
{
    body->vel(v);
    return ;
}		/* -----  end of method State::vel  ----- */

void
State::enu ( const UTM &utm )
{
    utm_c = utm.zone_c;
    utm_i = utm.zone_i;
    body->enu(utm);
    return ;
}		/* -----  end of method State::enu  ----- */

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

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

void
State::motionModel ( const Vector3d &acc, const Vector3d &ang,
    const Quaterniond &q, const double dt)
{
    Pkk1(ang,q,dt);
    body->motionModel(acc,ang,q,dt);
    return ;
}		/* -----  end of method State::motionModel  ----- */

void
State::position_covariance ( const Matrix<double,3,3> &p )
{
    P.block(0,0,3,3) = p;
    return ;
}		/* -----  end of method State::position_covariance  ----- */

void
State::velocity_covariance ( const Matrix<double,3,3> &p )
{
    P.block(3,3,3,3) = p;
    return ;
}		/* -----  end of method State::velocity_covariance  ----- */

void
State::unicsv ( )
{
    body->unicsv();
    return ;
}		/* -----  end of method State::unicsv  ----- */

void
State::accelerometer_bias ( const Vector3d &b)
{
    body->accelerometer_bias(b);
    return ;
}		/* -----  end of method State::accelerometer_bias  ----- */