/home/toby/moos-ivp/MOOS-2375-Oct0611/NavigationAndControl/MOOSNavLib/MOOSNavVehicle.cpp

Go to the documentation of this file.

//
//   MOOS - Mission Oriented Operating Suite 
//  
//   A suit of Applications and Libraries for Mobile Robotics Research 
//   Copyright (C) 2001-2005 Massachusetts Institute of Technology and 
//   Oxford University. 
//    
//   This software was written by Paul Newman and others
//   at MIT 2001-2002 and Oxford University 2003-2005.
//   email: pnewman@robots.ox.ac.uk. 
//      
//   This file is part of a  MOOS Basic (Common) Application. 
//        
//   This program is free software; you can redistribute it and/or 
//   modify it under the terms of the GNU General Public License as 
//   published by the Free Software Foundation; either version 2 of the 
//   License, or (at your option) any later version. 
//          
//   This program is distributed in the hope that it will be useful, 
//   but WITHOUT ANY WARRANTY; without even the implied warranty of 
//   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
//   General Public License for more details. 
//            
//   You should have received a copy of the GNU General Public License 
//   along with this program; if not, write to the Free Software 
//   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 
//   02111-1307, USA. 
//
// MOOSNavVehicle.cpp: implementation of the CMOOSNavVehicle class.
//
#include "MOOSNavLibGlobalHelper.h"
#include "MOOSNavVehicle.h"
#include "MOOSNavLibDefs.h"
#include "math.h"
// Construction/Destruction

CMOOSNavVehicle::CMOOSNavVehicle()
{
    m_eType = POSE_ONLY;
    

    m_bDroppedState = false;

}

CMOOSNavVehicle::~CMOOSNavVehicle()
{

}

bool CMOOSNavVehicle::GetFullState(Matrix &Result,Matrix * m_pXToUse,bool bUseEstimate)
{
    if(Result.Nrows()!=FULL_STATES    || Result.Ncols()!=1)
    {
        Result.ReSize(FULL_STATES,1);
    }

    //have we been told to use a vector other than the global state vector?
    if(m_pXToUse==NULL)
    {
        //yes (this may happen during numerical evaluation of jacobians
        //for example
        m_pXToUse = m_pXhat;
    }

    //set all to zero..
    Result    =0;
    
    if(bUseEstimate)
    {
        I_X(Result,1)=I_X((*m_pXToUse),m_nStart);
        I_Y(Result,1)=I_Y((*m_pXToUse),m_nStart);
        I_Z(Result,1)=I_Z((*m_pXToUse),m_nStart);
        I_H(Result,1)=I_H((*m_pXToUse),m_nStart);

        if(m_eType==POSE_AND_RATE)
        {
            I_Xdot(Result,1)=I_Xdot((*m_pXToUse),m_nStart);
            I_Ydot(Result,1)=I_Ydot((*m_pXToUse),m_nStart);
            I_Zdot(Result,1)=I_Zdot((*m_pXToUse),m_nStart);
            I_Hdot(Result,1)=I_Hdot((*m_pXToUse),m_nStart);
        }
    }
    else
    {
        I_X(Result,1)=m_State.m_dfX;
        I_Y(Result,1)=m_State.m_dfY;
        I_Z(Result,1)=m_State.m_dfZ;
        I_H(Result,1)=m_State.m_dfH;

        if(m_eType==POSE_AND_RATE)
        {
            I_Xdot(Result,1)=m_State.m_dfXdot;
            I_Ydot(Result,1)=m_State.m_dfYdot;
            I_Zdot(Result,1)=m_State.m_dfZdot;
            I_Hdot(Result,1)=m_State.m_dfHdot;
        }
    }

    return true;



}


int CMOOSNavVehicle::GetStateSize()
{
    switch(m_eType)
    {
        case POSE_ONLY:        return POSE_ONLY_STATES; break;
        case POSE_AND_RATE: return POSE_AND_RATE_STATES; break;
        default:
            return 0;
    }
}

bool CMOOSNavVehicle::RefreshState()
{


//    MOOSMatrixTrace(*m_pPhat,"Phat");
    m_State.m_dfX=I_X((*m_pXhat),m_nStart);
    m_State.m_dfY=I_Y((*m_pXhat),m_nStart);
    m_State.m_dfZ=I_Z((*m_pXhat),m_nStart);
    m_State.m_dfH=I_H((*m_pXhat),m_nStart);

    m_State.m_dfPX=(*m_pPhat)(m_nStart+iiX,m_nStart+iiX);
    m_State.m_dfPY=(*m_pPhat)(m_nStart+iiY,m_nStart+iiY);
    m_State.m_dfPZ=(*m_pPhat)(m_nStart+iiZ,m_nStart+iiZ);
    m_State.m_dfPH=(*m_pPhat)(m_nStart+iiH,m_nStart+iiH);


    if(m_eType==POSE_AND_RATE)
    {
        m_State.m_dfXdot=I_Xdot((*m_pXhat),m_nStart);
        m_State.m_dfYdot=I_Ydot((*m_pXhat),m_nStart);
        m_State.m_dfZdot=I_Zdot((*m_pXhat),m_nStart);
        m_State.m_dfHdot=I_Hdot((*m_pXhat),m_nStart);        


        m_State.m_dfPXdot=(*m_pPhat)(m_nStart+iiXdot,m_nStart+iiXdot);
        m_State.m_dfPYdot=(*m_pPhat)(m_nStart+iiYdot,m_nStart+iiYdot);
        m_State.m_dfPZdot=(*m_pPhat)(m_nStart+iiZdot,m_nStart+iiZdot);
        m_State.m_dfPHdot=(*m_pPhat)(m_nStart+iiHdot,m_nStart+iiHdot);


    }

    m_State.m_dfSpeed = sqrt(pow(m_State.m_dfXdot,2)+pow(m_State.m_dfYdot,2)+pow(m_State.m_dfZdot,2));

    m_State.m_dfDepth = (*m_pXhat)(TIDE_STATE_INDEX,1)-m_State.m_dfZ;
    return true;
}

bool CMOOSNavVehicle::FillModelMatrices(Matrix &F,Matrix &Q,Matrix & Xhat,double dfDeltaT)
{

    switch(m_eType)
    {
    case POSE_AND_RATE:
        return FillPoseAndRateModelMatrices(F,Q,Xhat,dfDeltaT);
        break;    
    case POSE_ONLY:
        return FillPoseOnlyModelMatrices(F,Q,Xhat,dfDeltaT);
        break;    
    default:
        MOOSTrace("err what kind of vehicle is this?\n");
        return false;
    }

}

bool CMOOSNavVehicle::SetDynamics(double dfDynamicsXY, double dfDynamicsZ, double dfDynamicsYaw)
{
    double dfSFX = dfDynamicsXY/10.0;
    double dfSFY = dfDynamicsXY/10.0;
    double dfSFZ = dfDynamicsZ/10.0;
    double dfSFH = dfDynamicsYaw/10.0;

    m_Noise.m_dfqXv = pow((dfSFX*FULL_SCALE_Q_VEL),2);
    m_Noise.m_dfqYv = pow((dfSFY*FULL_SCALE_Q_VEL),2);
    m_Noise.m_dfqZv = pow((dfSFZ*FULL_SCALE_Q_VEL),2);
    m_Noise.m_dfqHv = pow((dfSFH*FULL_SCALE_HEADING_VEL),2);


    m_Noise.m_dfqXa =dfSFX*FULL_SCALE_Q_ACC_XY;
    m_Noise.m_dfqYa =dfSFY*FULL_SCALE_Q_ACC_XY;
    m_Noise.m_dfqZa    =dfSFZ*FULL_SCALE_Q_ACC_Z;
    m_Noise.m_dfqHa    =dfSFH*FULL_SCALE_HEADING_ACC;

    return true;
}



bool CMOOSNavVehicle::FillPoseOnlyModelMatrices(Matrix &jF, Matrix &jQ, Matrix &Xhat, double dfDeltaT)
{

    //static vehicles don't move!
/*
    F = 1    0    0    0
        0    1    0    0    
        0    0    1    0
        0    0    0    1

    Q = v    0    0    0
        0    v    0    0
        0    0    v    0
        0    0    0    v

    where v = dT
    ( x= v * t -> x^2 = v^2 * dT^2 )

*/

    double dfT2 = pow(dfDeltaT,2);
    int iPx = m_nStart+iiX;
    int iPy    = m_nStart+iiY;
    int iPz = m_nStart+iiZ;
    int iPh = m_nStart+iiH;


    
    jQ(iPx,iPx) = m_Noise.m_dfqXv*dfT2;  //qv^2* dT^2
    jQ(iPy,iPy) = m_Noise.m_dfqYv*dfT2;
    jQ(iPz,iPz) = m_Noise.m_dfqZv*dfT2;
    jQ(iPh,iPh) = m_Noise.m_dfqHv*dfT2;


    jF(iPx,iPx) = 1.0;  
    jF(iPy,iPy) = 1.0;
    jF(iPz,iPz) = 1.0;
    jF(iPh,iPh) = 1.0;                

    return true;
}

bool CMOOSNavVehicle::FillPoseAndRateModelMatrices(Matrix &jF, Matrix &jQ, Matrix &Xhat, double dfDeltaT)
{




/*
        F =        |1    0    0    0    dT    0    0    0 |
                |0    1    0    0    0    dT    0    0 |    
                |0    0    1    0    0    0    dT    0 |    
                |0    0    0    1    0    0    0    dT|    
                |0    0    0    0    1    0    0    0 |    
                |0    0    0    0    0    1    0    0 |    
                |0    0    0    0    0    0    1    0 |    
                |0    0    0    0    0    0    0    1 |    
    */

    
    int iPx = m_nStart+iiX;
    int iPy    = m_nStart+iiY;
    int iPz = m_nStart+iiZ;
    int iPh = m_nStart+iiH;
    int iVx = m_nStart+iiXdot;
    int iVy    = m_nStart+iiYdot;
    int iVz = m_nStart+iiZdot;
    int iVh = m_nStart+iiHdot;

    int i = 0;
    for(i = m_nStart; i<=m_nEnd;i++)
    {
        jF(i,i) = 1.0;
    }


    for(i = 0; i<4;i++)
    {
        jF(m_nStart+i,m_nStart+i+4) = dfDeltaT;
    }

    

    // now sort out Q

    double dfT4 = pow(dfDeltaT,4)/4;
    double dfT3 = pow(dfDeltaT,3)/2;
    double dfT2 = pow(dfDeltaT,2);




    //do the diagonals first 
    jQ(iPx,iPx) = m_Noise.m_dfqXa*dfT4;  //qa^2* dT^4/4
    jQ(iPy,iPy) = m_Noise.m_dfqYa*dfT4;
    jQ(iPz,iPz) = m_Noise.m_dfqZa*dfT4;
    jQ(iPh,iPh) = m_Noise.m_dfqHa*dfT4;

    jQ(iVx,iVx) = m_Noise.m_dfqXa*dfT2;  //qa^2* dT^2
    jQ(iVy,iVy) = m_Noise.m_dfqYa*dfT2;
    jQ(iVz,iVz) = m_Noise.m_dfqZa*dfT2;
    jQ(iVh,iVh) = m_Noise.m_dfqHa*dfT2;
        

    //cross correlation terms
    jQ(iPx,iVx) = m_Noise.m_dfqXa*dfT3;  //qa^2 * dT^3 /3
    jQ(iPy,iVy) = m_Noise.m_dfqYa*dfT3;
    jQ(iPz,iVz) = m_Noise.m_dfqZa*dfT3;
    jQ(iPh,iVh) = m_Noise.m_dfqHa*dfT3;


    jQ(iVx,iPx) = m_Noise.m_dfqXa*dfT3;
    jQ(iVy,iPy) = m_Noise.m_dfqYa*dfT3;
    jQ(iVz,iPz) = m_Noise.m_dfqZa*dfT3;
    jQ(iVh,iPh) = m_Noise.m_dfqHa*dfT3;

//    MOOSMatrixTrace(jQ,"Q");
    
    return true;
}

//used to mark the vehicle as an 'old' vehicle location and
//not used actively in tracking...
bool CMOOSNavVehicle::SetDroppedState(bool bDropped)
{
    m_bDroppedState = bDropped;
    return true;
}