path: root/src/state.cpp

/*
 * =====================================================================================
 *
 *       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(Pxx());
    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);
}

void
State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &q)
{
    std::vector<measurement_t> featuresToAdd;
    for (auto i=z.begin(); i!=z.end(); ++i) {
        if (exists(i->id)) {
            ;
        } else {
            featuresToAdd.push_back(*i);
        }
    }
    addFeatures( featuresToAdd, q);

    return ;
}		/* -----  end of method State::feature_update  ----- */

void
State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q)
{
    // Add new features
    for (auto i=F.begin(); i!=F.end(); ++i) {
        if (features.size()>MAXFEATURES) break;
        // Create feature
        Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, -0.44);
        if (!f->sane()) {
            delete f;
            continue;
        }
        features.push_back(f);

        // Expand P
        expandP();
        
        // Initialize P values
        int j=features.size();
        Matrix<double,3,3> Pi;
        Pi = f->P0();
        initializePi(j,Pi);
    }
    return ;
}		/* -----  end of method State::addFeatures  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  State
 *      Method:  State :: initializePi
 * Description:  Zeros out the off-diagonal blocks and sets the Pii block to Pi.
 *--------------------------------------------------------------------------------------
 */
    void
State::initializePi ( int i, const Matrix<double,3,3> &Pi )
{
    int pt=9+3*(i-1);
    P.block(pt,0,3,P.cols()) = MatrixXd::Zero(3,P.cols());
    P.block(0,pt,P.rows(),3) = MatrixXd::Zero(P.rows(),3);
    P.block<3,3>(pt,pt) = Pi;

    return ;
}		/* -----  end of method State::initializePi  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  State
 *      Method:  State :: Q
 * Description:  Compose full Q from body and features.
 *--------------------------------------------------------------------------------------
 */
MatrixXd
State::Q ( double dt )
{
    MatrixXd q;
    int rows = 9 + 3*features.size();
    q = MatrixXd::Zero(rows,rows);
    q.topLeftCorner<9,9>() = body->Q(dt);
    { // limit i's scope
        auto i = features.begin();
        for (int row=9; row<rows; row+=3, ++i) {
            q.block<3,3>(row,row) = (*i)->Q(dt);
        }
    }
    return q;
}		/* -----  end of method State::q  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  State
 *      Method:  State :: F
 * Description:  Compose full F from body and features
 *--------------------------------------------------------------------------------------
 */
MatrixXd
State::F ( const Quaterniond &q, const Vector3d &w, double dt )
{
    Vector3d v;
    v = body->vel();
    // Allocate matrix F
    MatrixXd f;
    int rows = 9 + 3*features.size();
    f = MatrixXd::Zero(rows,rows);

    // Set body F
    f.topLeftCorner<9,9>() = body->F(w,q,dt);
    // Set Fxi Fyi
    { // limit i's scope
        auto i = features.begin();
        for (int row=9; row<rows; row+=3,++i) {
            f.block<3,9>(row,0) = (*i)->Fx(dt);
            f.block<3,3>(row,row) = (*i)->Fy(v,w,dt);
        }
    }

    return f ;
}		/* -----  end of method State::F  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  State
 *      Method:  State :: Pkk1
 * Description:  Updates P_k|k-1
 *--------------------------------------------------------------------------------------
 */
void
State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt)
{
    MatrixXd f,fullQ;
    fullQ = Q(dt);
    f = F ( q, ang, dt );
    P = f*P*f.transpose()+fullQ;
    // 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);

    Vector3d vel;
    vel = body->vel();
    
#ifdef  FASTMOTIONMODEL
    Matrix<double,Dynamic,6> fullL;
    fullL = L();

    Matrix<double,6,1> V;
    V << vel[1], vel[2], vel[0], ang[1], ang[2], ang[0];

    Matrix<double,Dynamic,1> f;
    f = fullL*V*dt;

    // Update features' states
    {
        int rows = 3*features.size();
        auto i = features.begin();
        for (int row=0; row<rows; row+=3, ++i) {
            Vector3d dx;
            dx = f.segment<3>(row);
            (*i)->dx(dx);
        }
    }
    
#else      /* -----  not FASTMOTIONMODEL  ----- */
    for (auto i=features.begin(); i!=features.end(); ++i) {
        (*i)->motionModel(ang,vel,dt);
    }
#endif     /* -----  not FASTMOTIONMODEL  ----- */

    // Remove features that leave FOV
    {
        auto i = features.begin();
        int j=0;
        while (i!=features.end()) {
            if ((*i)->inFOV()) {
                ++i;
                ++j;
            } else {
                i=removeFeature(i,j);
            }
        }
    }


    return ;
}		/* -----  end of method State::motionModel  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  State
 *      Method:  State :: removeFeature
 * Description:  Remove the Feature i and Pj in a clean fashion.
 *--------------------------------------------------------------------------------------
 */
std::list<Feature *>::iterator
State::removeFeature ( std::list<Feature *>::iterator &i, int j )
{
    // It is important to shrink P first, because it depends on knowing the
    // current size of the feature vector.
    shrinkP(j);
    delete *i;
    i = features.erase(i);
    return i;
}		/* -----  end of method State::removeFeature  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  State
 *      Method:  State :: L
 * Description:  Return the composition of all Li's of the features
 *--------------------------------------------------------------------------------------
 */
Matrix<double,Dynamic,6>
State::L (  )
{
    Matrix<double,Dynamic,6> l;
    int rows = 3*features.size();
    l = Matrix<double,Dynamic,6>::Zero(rows,6);
    {
        auto i=features.begin();
        for (int row=0; row<rows; row+=3, ++i) {
            l.block<3,6>(row,0) = (*i)->L();
        }
    }
    return l;
}		/* -----  end of method State::L  ----- */

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

/*
 *--------------------------------------------------------------------------------------
 *       Class:  State
 *      Method:  State :: exists
 * Description:  Tests if the id is a current feature.
 *--------------------------------------------------------------------------------------
 */
bool
State::exists ( int id )
{
    for (auto i=features.begin(); i!=features.end(); ++i) {
        if ((*i)->id()==id) return true;
    }
    return false;
}		/* -----  end of method State::exists  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  State
 *      Method:  State :: expandP
 * Description:  Expand the P matrix to fit one more feature.
 *--------------------------------------------------------------------------------------
 */
void
State::expandP ( )
{
    P.conservativeResize(3+P.rows(),3+P.cols());
    return ;
}		/* -----  end of method State::expandP  ----- */

Matrix<double,9,9>
State::Pxx ( )
{
    return P.topLeftCorner<9,9>() ;
}		/* -----  end of method State::Pxx  ----- */

/*
 *--------------------------------------------------------------------------------------
 *       Class:  State
 *      Method:  State :: shrinkP
 * Description:  Delete the ith feature from P
 *--------------------------------------------------------------------------------------
 */
void
State::shrinkP ( int i )
{
    int N = 9 + 3*features.size();
    int I = 9 + 3*i;

    P.block(I,0,N-I-3,I) = P.bottomLeftCorner(N-I-3,I);
    P.block(0,I,I,N-I-3) = P.topRightCorner(I,N-I-3);
    P.block(I,I,N-I-3,N-I-3) = P.bottomRightCorner(N-I-3,N-I-3);
    P.conservativeResize(N-3,N-3);

    return ;
}		/* -----  end of method State::shrinkP  ----- */