/home/toby/moos-ivp/MOOS-2375-Oct0611/NavigationAndControl/MOOSNavLib/MOOSNavEKFEngine.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. 
//
// MOOSNavEKFEngine.cpp: implementation of the CMOOSNavEKFEngine class.
//
#include "MOOSNavLibGlobalHelper.h"
#include "MOOSNavLibDefs.h"
#include "MOOSNavObsStore.h"
#include "MOOSNavVehicle.h"
#include "MOOSNavEKFEngine.h"

#define MAX_ALLOWED_LIN_VELOCITY 3.0
#define MAX_ALLOWED_ANG_VELOCITY MOOSDeg2Rad(15)
#define EKF_ITERATE_PERIOD 0.3
#define TIME_SLICE (EKF_ITERATE_PERIOD/2.0)

// Construction/Destruction

CMOOSNavEKFEngine::CMOOSNavEKFEngine()
{
    m_dfXYDynamics=5;
    m_dfZDynamics=5;
    m_dfYawDynamics=5;    
    
    m_dfPxx0    = pow(30.0,2.0);
    m_dfPyy0    = pow(30.0,2.0);
    m_dfPzz0    = pow(10.0,2.0);
    m_dfPhh0    = pow(MOOSDeg2Rad(90),2.0);
    
    m_dfX0 = 0;
    m_dfY0 = 0;
    m_dfZ0 = 50;
    m_dfH0 = 0;
    m_dfTide0 = 50;
    
    m_dfLastIterated = 0;
    
    m_dfLag = 2.0;
    
    m_sName = "EKF";
    
    m_bBooted = 0;

    m_dfYawBiasStd = 0;
    
    m_dfMaxZVel = MAX_ALLOWED_LIN_VELOCITY;

    m_bInitialOnline = false;
}

CMOOSNavEKFEngine::~CMOOSNavEKFEngine()
{
    
}


bool CMOOSNavEKFEngine::Initialise(STRING_LIST  sParams)
{
    
    //before calling base class member peek to see if we
    //are being told to modify the defaul kind of vehcile that
    //will be built
    string sVal;
    if(MOOSGetValueFromToken(sParams,"EKF_VEHICLE_TYPE",sVal))
    {
        if(MOOSStrCmp(sVal,"MOBILE"))
        {
            m_eVehicleType =  CMOOSNavEntity::POSE_AND_RATE;
        }
    }

    //do we want to estimate heading bias?
    if(MOOSGetValueFromToken(sParams,"EKF_ESTIMATE_YAW_BIAS",sVal))
    {
        if(MOOSStrCmp(sVal,"TRUE"))
        {
            m_bEstimateHeadingBias = true;
        }
    }



    //now call base class version
    if(!CMOOSNavEngine::Initialise(sParams))
    {
        return false;
    }
    

       if(MOOSGetValueFromToken(sParams,"EKF_YAW_BIAS_NOISE",sVal))
    {
        m_dfYawBiasStd = atof(sVal.c_str());
        if(m_dfYawBiasStd<0) 
            m_dfYawBiasStd=0;
    }

    
    if(MOOSGetValueFromToken(sParams,"EKF_LAG",sVal))
    {
        double dfVal = atof(sVal.c_str());
        if(dfVal<0) dfVal=0;
        if(dfVal>4) dfVal = 4;
        m_dfLag = dfVal;
    }
    


    

    //now we may have other things to set up....
    if(MOOSGetValueFromToken(sParams,"EKF_XY_DYNAMICS",sVal))
    {
        double dfVal = atof(sVal.c_str());
        if(dfVal<0) dfVal=0;
        if(dfVal>10) dfVal = 10;
        m_dfXYDynamics = dfVal;
    }
    
    if(MOOSGetValueFromToken(sParams,"EKF_Z_DYNAMICS",sVal))
    {
        double dfVal = atof(sVal.c_str());
        if(dfVal<0) dfVal=0;
        if(dfVal>10) dfVal = 10;
        m_dfZDynamics = dfVal;
    }
    
    if(MOOSGetValueFromToken(sParams,"EKF_YAW_DYNAMICS",sVal))
    {
        double dfVal = atof(sVal.c_str());
        if(dfVal<0) dfVal=0;
        if(dfVal>10) dfVal = 10;
        m_dfYawDynamics = dfVal;
    }
    
    
    //rad in intial uncertainties
    if(MOOSGetValueFromToken(sParams,"EKF_SIGMA_XX",sVal))
    {
        double dfVal = atof(sVal.c_str());
        if(dfVal>0)
        {
            m_dfPxx0 = pow(dfVal,2);
        }
    }
    
    if(MOOSGetValueFromToken(sParams,"EKF_SIGMA_YY",sVal))
    {
        double dfVal = atof(sVal.c_str());
        if(dfVal>0)
        {
            m_dfPyy0 = pow(dfVal,2);
        }
    }
    
    if(MOOSGetValueFromToken(sParams,"EKF_SIGMA_ZZ",sVal))
    {
        double dfVal = atof(sVal.c_str());
        if(dfVal>0)
        {
            m_dfPzz0 = pow(dfVal,2);
        }
    }
    
    if(MOOSGetValueFromToken(sParams,"EKF_SIGMA_HH",sVal))
    {
        double dfVal = atof(sVal.c_str());
        if(dfVal>0)
        {
            m_dfPhh0 = pow(MOOSDeg2Rad(dfVal),2);
        }
    }
    
    if(MOOSGetValueFromToken(sParams,"EKF_SIGMA_TIDE",sVal))
    {
        double dfVal = atof(sVal.c_str());
        if(dfVal>0)
        {
            m_dfPTide0 = pow((dfVal),2);
        }
    }
    
    
    //where shall we start from?
    if(MOOSGetValueFromToken(sParams,"EKF_X",sVal))
    {
        double dfVal = atof(sVal.c_str());
        m_dfX0 = dfVal;
    }
    if(MOOSGetValueFromToken(sParams,"EKF_Y",sVal))
    {
        double dfVal = atof(sVal.c_str());
        m_dfY0 = dfVal;
    }
    if(MOOSGetValueFromToken(sParams,"EKF_Z",sVal))
    {
        double dfVal = atof(sVal.c_str());
        m_dfZ0 = dfVal;
    }
    if(MOOSGetValueFromToken(sParams,"EKF_H",sVal))
    {
        double dfVal = atof(sVal.c_str());
        //convert to yaw in degrees
        m_dfH0 = MOOSDeg2Rad(-dfVal);
    }
    if(MOOSGetValueFromToken(sParams,"EKF_TIDE",sVal))
    {
        double dfVal = atof(sVal.c_str());
        m_dfTide0 = dfVal;
    }
    if(MOOSGetValueFromToken(sParams,"EKF_MAX_Z_VEL",sVal))
    {
        double dfVal = atof(sVal.c_str());
        m_dfMaxZVel = dfVal;
    }



    m_pTracked->m_State.m_dfX = m_dfX0;
    m_pTracked->m_State.m_dfY = m_dfY0;
    m_pTracked->m_State.m_dfZ = m_dfZ0;
    m_pTracked->m_State.m_dfH = m_dfH0;
    
    m_pTracked->m_State.m_dfPX = m_dfPxx0;
    m_pTracked->m_State.m_dfPY = m_dfPyy0;
    m_pTracked->m_State.m_dfPZ = m_dfPzz0;
    m_pTracked->m_State.m_dfPH = m_dfPhh0;
    
    m_pTracked->m_State.m_dfPXdot = 0.0;
    m_pTracked->m_State.m_dfPYdot = 0.0;
    m_pTracked->m_State.m_dfPZdot = 0.0;
    m_pTracked->m_State.m_dfPHdot = 0.0;



    m_pStore->m_sOwnerName="EKF";
    
    m_nUpdates = 0;
    
    return true;
}


bool CMOOSNavEKFEngine::AddData(const CMOOSMsg &Msg)
{
    
    //    if(Msg.m_sKey=="LBL_TOF")
    //    {
    //        MOOSTrace("EKF gets LBL\n");
    //    }
    return CMOOSNavEngine::AddData(Msg);
}


bool CMOOSNavEKFEngine::Iterate(double dfTimeNow)
{
    
    //not allowed to run if not Online!
    if(!IsOnline())
        return true;
    
    if(dfTimeNow-m_dfLastIterated<EKF_ITERATE_PERIOD)
        return true;
    
    m_dfLastIterated = dfTimeNow;
    
    //keep a copy of the time..
    m_dfTimeNow = dfTimeNow;
    
    if(!IsBooted())
    {
        return Boot();
    }
    else
    {
        
        //what time should we stop at?
        double dfTStop = m_dfTimeNow-m_dfLag;
        
        double dfNewestObsTime = m_pStore->GetNewestObsTime();
        
        if(dfNewestObsTime<=m_dfLastUpdate)
        {
            //predict only ..no new observations have come in...
            double dfDeltaT = dfTStop-m_dfLastUpdate;
            if(!DoPredict(dfDeltaT))
                return false;    
        }
        else
        {
            //get observations in small batches that are close to
            //each other...
            
            double dfDT = dfTStop-m_dfLastUpdate;
            
            int nIterations = 0;
            while(dfDT>TIME_SLICE)
            {
                //split up the whole period into even sized chunks
                dfDT=(dfTStop-m_dfLastUpdate)/(++nIterations);
            }
            
            //for each of these chuncks predict then look to update...
            for(int nIteration = 0;nIteration<nIterations;nIteration++)
            {
                if(!DoPredict(dfDT))
                    return false;
                
                m_Observations.clear();
                
                double dfT1 = m_dfLastUpdate+nIteration*dfDT;
                double dfT2 = dfT1+dfDT;
                
                //get the observations (close to each other in time)
                //MOOSTrace("EKF @ %.3f true time = %.3f\n",dfTimeNow,MOOSTime());
                if(m_pStore->GetObservationsBetween(m_Observations,dfT1,dfT2))
                {
                    if(!m_Observations.empty())
                    {
                        
                        //TraceObservationSet();
                        
                        if(nIteration==0)
                        {
                            //here we add in the fixed observations....
                            m_Observations.insert(    m_Observations.begin(),
                                m_FixedObservations.begin(),
                                m_FixedObservations.end());
                        }
                        
                        //limit the number of certain kinds of observations
                        LimitObservations(CMOOSObservation::DEPTH,1);
                        LimitObservations(CMOOSObservation::YAW,1);
                        
                        bool bDone = false;

                        //safety - we should always be removeing at least one observation
                        //per DA loop so limit the number of times we can do this!
                        int nDACycles = m_Observations.size();
                        while(!bDone && (nDACycles--)>0)
                        {
                            if(MakeObsMatrices())
                            {
                                
                                //calcuate innovation covariance
                                try
                                {
                                    if(m_Innov.Nrows()<0)
                                    {
                                        //exit condition for DA
                                        bDone = true;
                                        continue;
                                    }

                                    //Kalman equation for innovation covariance
                                    m_S = m_jH*m_Phat*m_jH.t() + m_R;

                                    RecordObsStatistics(&m_Innov,&m_S);
                                    
                                    //do data rejection...
                                    bool bAccepted = true;
                                    if(MOOSChiSquaredTest(m_Innov,m_S,false))
                                    {
                                        Matrix W = m_Phat*m_jH.t()*m_S.i();
                                        m_Phat-=W*m_S*W.t();
                                        m_Xhat+=W*m_Innov;

                                        //complete....
                                        bDone = true;   
                                    }
                                    else
                                    {
                                        if(m_Innov.Nrows()>1)
                                        {
                                            //lets figure out which observation was hurting us...
                                            Matrix Si = m_S.i();
                                            int iReject = HyperDimSelect(m_Innov,m_S,Si);
                                            
                                            //now we need to figure out what observation that was
                                            //and set its bGoodDA to false!
                                            OBSLIST::iterator t;
                                            for(t = m_Observations.begin();t!=m_Observations.end();t++)
                                            {
                                                if(t->m_bIgnore==false && t->m_nRow==iReject)
                                                {
                                                    t->SetGoodDA(false);
                                                    MOOSTrace("Hyperdim Select removes obs %d \n",
                                                                t->GetID());
                                                    t->Trace();
                                                    break;
                                                }
                                            }
                                        }
                                    }                                    
                                }
                                catch(...)
                                {
                                    AddToOutput("Trouble:EKF inversion failed!\n");
                                    MOOSMatrixTrace(m_jH,"H");
                                }
                            }
                        }
                        m_nUpdates++;
                                                    
                        LogObservationSet((dfT1+dfT2)/2,
                            m_nUpdates);

                    }
                }
            }
        }        
        
        OnIterateDone(dfTStop);
    }
    return true;
}

bool CMOOSNavEKFEngine::DoPredict(double dfDeltaT)
{
    
    if(!PreparePredictionMatrices())
        return false;
    
    if(!m_pTracked->FillModelMatrices(m_jF,m_jQ,m_Xhat,dfDeltaT))
        return false;
    
    if(!FillGlobalParamModelMatrices(dfDeltaT))
        return false;
    
    //  KALMAN PREDICTION 
    //
    // note that in this simple case we have 
    // a linear model change this later
    //
    
    //    MOOSMatrixTrace(m_jF,"F");
    //    MOOSMatrixTrace(m_jQ,"Q");
    
    
    m_Xhat = m_jF*m_Xhat;
    m_Phat = m_jF*m_Phat*(m_jF.t()) + m_jQ;
    
    
    return true;
}

bool CMOOSNavEKFEngine::FillGlobalParamModelMatrices(double dfDeltaT)
{
    //firstly do tide
    m_jF(TIDE_STATE_INDEX,TIDE_STATE_INDEX) = 1;
    m_jQ(TIDE_STATE_INDEX,TIDE_STATE_INDEX) = pow(2e-4,2); //noise on tide...~ 9m in 12 hours
    
    if(m_bEstimateHeadingBias)
    {
        //optionally do heading bias
        m_jF(HEADING_BIAS_STATE_INDEX,HEADING_BIAS_STATE_INDEX) = 1;
        m_jQ(HEADING_BIAS_STATE_INDEX,HEADING_BIAS_STATE_INDEX) = pow(m_dfYawBiasStd,2); 
    }

    return true;
}


bool CMOOSNavEKFEngine::Boot()
{
    CMOOSNavVehicle * pVeh = dynamic_cast< CMOOSNavVehicle* > (m_pTracked);
    
    if(pVeh==NULL)
        return false;
    
    //set up vehcile mobility/dynamics..
    pVeh->SetDynamics(m_dfXYDynamics,m_dfZDynamics,m_dfYawDynamics);
    
    //set up initial conditions
    InitialiseEstimates();
    
    //start the clock
    m_dfLastUpdate = m_dfTimeNow;
    
    m_bBooted = true;
    
    return m_bBooted;
}


bool CMOOSNavEKFEngine::InitialiseEstimates()
{
    
    m_Phat = 0;
    m_Xhat = 0;
    
    //do tide first
    m_Phat(TIDE_STATE_INDEX,TIDE_STATE_INDEX) = m_dfPTide0;
    m_Xhat(TIDE_STATE_INDEX,1) = m_dfTide0;
    
    
    
    
    m_pTracked->RefreshStateVector();
    
    m_pTracked->RefreshStateCovariance();
       
    
    return true;
}

bool CMOOSNavEKFEngine::IsBooted()
{
    return m_bBooted;
}

bool CMOOSNavEKFEngine::PreparePredictionMatrices()
{
    if(m_jF.Nrows()!=m_Xhat.Nrows() || m_jF.Ncols()!=m_Xhat.Nrows())
    {
        m_jF.ReSize(m_Xhat.Nrows(),m_Xhat.Nrows());
    }
    
    if(m_jQ.Nrows()!=m_Xhat.Nrows() || m_jQ.Ncols()!=m_Xhat.Nrows())
    {
        m_jQ.ReSize(m_Xhat.Nrows(),m_Xhat.Nrows());
    }
    
    //zero all matrices before we start
    m_jQ = 0;
    m_jF = 0;
    
    return true;
}

bool CMOOSNavEKFEngine::OnIterateDone(double dfUpdateTime)
{
    //remeber our success!
    m_dfLastUpdate = dfUpdateTime;
    
    //limit velocities
    LimitVelocityStates();
    
    //look after PI
    WrapAngleStates();
    
    MakeSymmetric();
    
    //copy estimates 
    m_XTmp = m_Xhat;
    m_PTmp = m_Phat;
    
    
    //predict forwards
    PredictForward(dfUpdateTime,m_dfTimeNow);
    
    //we won;t predict the covariance forwads for siplay purposes
    m_Phat = m_PTmp;
    
    //look after PI
    WrapAngleStates();
    
    if(m_pTracked->RefreshState())
    {
        m_nIterations++;
        PublishResults();
        
        m_Logger.LogState(m_dfTimeNow,m_Xhat,m_Phat);
    }
    
    //copy estimate back (things went really well we want to
    //keep our results...
    m_Xhat = m_XTmp;


    //now we keep statistics on observations
    
    
    
    return true;
}

bool CMOOSNavEKFEngine::PublishResults()
{
    //MOOSMatrixTrace(m_Phat,"PHat");

//    TraceDiagPhat();

    //Ok so lets make some results..
    CMOOSMsg MsgX(MOOS_NOTIFY,"EKF_X", m_pTracked->m_State.m_dfX,m_dfTimeNow);
    m_ResultsList.push_front(MsgX);
    
    CMOOSMsg MsgY(MOOS_NOTIFY,"EKF_Y", m_pTracked->m_State.m_dfY,m_dfTimeNow);
    m_ResultsList.push_front(MsgY);
    
    CMOOSMsg MsgZ(MOOS_NOTIFY,"EKF_Z", m_pTracked->m_State.m_dfZ,m_dfTimeNow);
    m_ResultsList.push_front(MsgZ);
    
    CMOOSMsg MsgYaw(MOOS_NOTIFY,"EKF_YAW", m_pTracked->m_State.m_dfH,m_dfTimeNow);
    m_ResultsList.push_front(MsgYaw);
    
    CMOOSMsg MsgDepth(MOOS_NOTIFY,"EKF_DEPTH", m_pTracked->m_State.m_dfDepth,m_dfTimeNow);
    m_ResultsList.push_front(MsgDepth);
    
    CMOOSMsg MsgTide(MOOS_NOTIFY,"EKF_TIDE", m_Xhat(TIDE_STATE_INDEX,1),m_dfTimeNow);
    m_ResultsList.push_front(MsgTide);
    
    if(m_pTracked->GetEntityType()==CMOOSNavEntity::POSE_AND_RATE)
    {
        CMOOSMsg MsgX(MOOS_NOTIFY,"EKF_X_VEL", m_pTracked->m_State.m_dfXdot,m_dfTimeNow);
        m_ResultsList.push_front(MsgX);
        
        CMOOSMsg MsgY(MOOS_NOTIFY,"EKF_Y_VEL", m_pTracked->m_State.m_dfYdot,m_dfTimeNow);
        m_ResultsList.push_front(MsgY);
        
        CMOOSMsg MsgZ(MOOS_NOTIFY,"EKF_Z_VEL", m_pTracked->m_State.m_dfZdot,m_dfTimeNow);
        m_ResultsList.push_front(MsgZ);
        
        CMOOSMsg MsgYaw(MOOS_NOTIFY,"EKF_YAW_VEL", m_pTracked->m_State.m_dfHdot,m_dfTimeNow);
        m_ResultsList.push_front(MsgYaw);
        
        CMOOSMsg MsgSpeed(MOOS_NOTIFY,"EKF_SPEED", m_pTracked->m_State.m_dfSpeed,m_dfTimeNow);
        m_ResultsList.push_front(MsgSpeed);
        
    }

    //have we been estiamting bias on heading?
    if(this->m_bEstimateHeadingBias)
    {
        CMOOSMsg MsgBias(MOOS_NOTIFY,"EKF_YAW_BIAS", m_Xhat(HEADING_BIAS_STATE_INDEX,1),m_dfTimeNow);
        m_ResultsList.push_front(MsgBias);
        
    }
    
    
    string sCov = MOOSFormat("SigmaX = %f SigmaY = %f",
        sqrt(m_pTracked->m_State.m_dfPX),
        sqrt(m_pTracked->m_State.m_dfPY));
    
    CMOOSMsg MsgCov(MOOS_NOTIFY,"EKF_SIGMA",sCov.c_str(),m_dfTimeNow);
    m_ResultsList.push_front(MsgCov);

    DoObservationStatistics();
    
    if(m_nIterations%100==0)
    {
        string sResult = MOOSFormat("EKF Completes iteration %d",m_nIterations);
        AddToOutput(sResult);

        DoObservationSummary();
    }
    


    return true;
}

bool CMOOSNavEKFEngine::PredictForward(double dfStop, double dfTimeNow)
{
    //so have stopped at time dfStop but want a result at time dfTimeNow
    //->Forward Predict
    double dfDT = dfTimeNow-dfStop;
    
    //MOOSTrace("Forward Predicting by %7.2f seconds:\n",dfDT); 
    
    return DoPredict(dfDT);
    
}

bool CMOOSNavEKFEngine::MakeSymmetric()
{
    for(int i = 1;i<m_Phat.Nrows();i++)
    {
        for(int j = i;j<m_Phat.Nrows();j++)
        {
            m_Phat(i,j) = m_Phat(j,i);
        }
    }
    
    return true;
}

bool CMOOSNavEKFEngine::Reset()
{
    
    m_bBooted = false;
    
    //try to reboot....
    Boot();
    
    if(m_bBooted)
    {
        AddToOutput("EKF Reset: OK");
    }
    else
    {
        AddToOutput("EKF Reset: FAILED");
    }
    
    return m_bBooted;
}

bool CMOOSNavEKFEngine::LimitVelocityStates()
{
    
    
    if(m_pTracked->GetEntityType()==CMOOSNavEntity::POSE_AND_RATE)
    {
        double dfXdot = (*m_pTracked->m_pXhat)(m_pTracked->m_nStart+iiXdot,1);
        if(fabs(dfXdot)>MAX_ALLOWED_LIN_VELOCITY)
        {
            AddToOutput("EKF X Vel is too large (divergence?)-> resetting");
            (*m_pTracked->m_pXhat)(m_pTracked->m_nStart+iiXdot,1)=0;
        }
        
        double dfYdot = (*m_pTracked->m_pXhat)(m_pTracked->m_nStart+iiYdot,1);
        if(fabs(dfYdot)>MAX_ALLOWED_LIN_VELOCITY)
        {
            AddToOutput("EKF Y Vel is too large (divergence?)-> resetting");
            (*m_pTracked->m_pXhat)(m_pTracked->m_nStart+iiYdot,1)=0;
        }
        
        double dfZdot = (*m_pTracked->m_pXhat)(m_pTracked->m_nStart+iiZdot,1);
        if(fabs(dfZdot)>m_dfMaxZVel)
        {
            if(m_dfMaxZVel>0)
            {
                AddToOutput("EKF Z Vel is too large (divergence?)-> resetting");
            }
            (*m_pTracked->m_pXhat)(m_pTracked->m_nStart+iiZdot,1)=0;
        }
        
        double dfHdot = (*m_pTracked->m_pXhat)(m_pTracked->m_nStart+iiHdot,1);
        if(fabs(dfHdot)>MAX_ALLOWED_ANG_VELOCITY)
        {
            AddToOutput("EKF YAW Vel is too large (divergence?)-> resetting");
            (*m_pTracked->m_pXhat)(m_pTracked->m_nStart+iiHdot,1)=0;
        }
        
    }
    
    return true;
}


int  CMOOSNavEKFEngine::HyperDimSelect(Matrix & Innov,Matrix & Cov,Matrix & InvCov)
{
    int nDimension = Innov.Nrows();
    
    Matrix    ZUpper    = Innov;
    Matrix    ZLower    = Innov;
    ZLower    = 0.0;
    Matrix    ZTmp    = ZLower;
    
    //make sure Zupper is outside
    if(IsInside(ZUpper,InvCov))
    {
        return -1;
    }
    
    
    
    Matrix  Tmp        = (ZUpper-ZLower);
    Matrix    Tmp2    = Tmp.t()*Tmp;
    double Dist = sqrt(Tmp2(1,1));
    
    int nIt = 0;
    while(Dist>0.04)
    {
        ZTmp = (ZUpper+ ZLower)/2;
        
        if(IsInside(ZTmp,InvCov))
        {
            ZLower = ZTmp;
        }
        else
        {
            ZUpper = ZTmp;
        }
        
        //vector between bracing solutions
        Tmp = (ZUpper-ZLower);
        Tmp2 = Tmp.t()*Tmp;
        
        //length of vector
        Dist = sqrt(Tmp2(1,1));
        
        if(++nIt>1000)
        {
            return -1;
        }
        
    }
    
    //
    //    OK so ZTmp is now the intersection of
    //    the hyper ellpsoid with the innovation vector
    //    find the maximum component ratios
    //
    
    
    double MaxRatio =0.0;
    int        iReject = -1;
    for(int i = 1; i<=nDimension;i++)
    {
        double dfTmp =fabs((Innov(i,1)-ZTmp(i,1))/Cov(i,i));
        
        if(dfTmp>=MaxRatio)
        {
            MaxRatio = dfTmp;
            //this obs to be rejected..
            iReject = i;
        }
    }
    
    
    return iReject;
    
}
bool CMOOSNavEKFEngine::IsInside(Matrix &v, Matrix &Ellipse)
{
    Matrix Tmp = v.t()*Ellipse*v;
    if(Tmp(1,1) <1.0)
        return true;
    else
        return false;
}