File content as of revision 43:cbc2c2d65131:

#include "ScErrStateEKF.hpp"
#include "Matrix.h"
#include "MatrixMath.h"
#include <cmath>
#include "Vector3.hpp"


using namespace std;

ScErrStateEKF::ScErrStateEKF()
    :qhat(4,1),vihat(3,1), errState(12,1),accned(3,1), Phat(12,12), Q(12,12),Ra(3,3), Rgps(3,3),Rsr(1,1), Rm(3,3), Qab(3,3),Rgsc(2,2),Rvsc(2,2),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f)
{ 
    nState = errState.getRows();
    qhat << 1.0f << 0.0f << 0.0f << 0.0f;
    vihat << 0.0f << 0.0f << 0.0f;
    for (int i = 1; i <= nState; i++)
    {
        errState(i, 1) = 0.0f;
    }
    
    setDiag(Phat,0.1f); 
    Phat(4,4) = 0.001;
    Phat(5,5) = 0.001;
    Phat(6,6) = 0.001;
    Phat(7,7) = 1.0;
    Phat(8,8) = 1.0;
    Phat(9,9) = 1.0;
    Phat(10,10) = 1.0;
    Phat(11,11) = 1.0;
    Phat(12,12) = 1.0;
    setQqerr(0.001f);  
    setQgbias(0.0001f);  
    setQabias(1.0f);
    setQv(1.0f);
    
    //加速度の観測
    setDiag(Ra,0.1f);
    setDiag(Qab,1.5f);  
    
    //ジャイロバイアスに関する制約
    setDiag(Rgsc,500.0f);  
    
    //地磁気（未使用）
    setDiag(Rm,5.0f);
    
    //GPS
    setDiag(Rgps,10.0f);
    //降下速度
    setDiag(Rsr,0.001f);
    
    for(int i = 0; i<10;i++){
        histffunc[i] = 0.0f;
    }
    histffuncindex = 0 ;
    sigma2a = 0.000020f;
    
    
}

void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt)
{
    gyro -= gyroBias;
    //acc -= accBias;
    Matrix A(4,4);
    A <<  0.0f        << -0.5f*gyro.x <<-0.5f*gyro.y <<-0.5f*gyro.z
      <<  0.5f*gyro.x <<  0.0f        << 0.5f*gyro.z <<-0.5f*gyro.y
      <<  0.5f*gyro.y << -0.5f*gyro.z << 0.0f        << 0.5f*gyro.x
      <<  0.5f*gyro.z <<  0.5f*gyro.y <<-0.5f*gyro.x << 0.0f ;
        
    Matrix phi = MatrixMath::Eye(4) + A * att_dt + (0.5f*att_dt*att_dt) * A * A;
    qhat = phi * qhat;
    float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
    qhat *= (1.0f/ qnorm);
    
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    accned = (MatrixMath::Transpose(dcm)*MatrixMath::Vector2mat(acc)-MatrixMath::Vector2mat(accref));
    vihat += accned*att_dt*9.8f;
}

void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt)
{
    gyro -= gyroBias;
    //acc -= accBias;
    Matrix A(nState,nState);
    A(1,2) =  gyro.z;
    A(1,3) = -gyro.y;
    A(2,1) = -gyro.z;
    A(2,3) =  gyro.x;
    A(3,1) =  gyro.y;
    A(3,2) = -gyro.x;
    A(1,4) =  -0.5f;
    A(2,5) =  -0.5f;
    A(3,6) =  -0.5f;
    
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix qeterm = -2.0f*9.8f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(acc.x,acc.y,acc.z);
    //Matrix baterm = -9.8f*MatrixMath::Transpose(dcm);
    for (int i = 1; i < 4; i++){
        for (int j = 1; i < 4; i++){
            A(i+9,j) = qeterm(i,j);
            //A(i+9,j+6) = baterm(i,j);
        }
    }
    
    Matrix phi = MatrixMath::Eye(nState) + A * att_dt + (0.5f*att_dt*att_dt) * A * A;
    Matrix Qd = Q * att_dt + 0.5f*A*Q * att_dt * att_dt+ 0.5f*Q*MatrixMath::Transpose(A) * att_dt * att_dt;
    errState = phi * errState;
    Phat = phi*Phat*MatrixMath::Transpose(phi)+Qd;
}
void ScErrStateEKF::setQqerr(float val){
    Q(1,1) = val;
    Q(2,2) = val; 
    Q(3,3) = val;
}

void ScErrStateEKF::setQgbias(float val){
    Q(4,4) = val;
    Q(5,5) = val; 
    Q(6,6) = val;
}
void ScErrStateEKF::setQabias(float val){
    Q(7,7) = val;
    Q(8,8) = val; 
    Q(9,9) = val;
}
void ScErrStateEKF::setQv(float val){
    Q(10,10) = val;
    Q(11,11) = val; 
    Q(12,12) = val;
}

void ScErrStateEKF::setQab(float val){
    setDiag(Qab,val);
}


void ScErrStateEKF::setRsoftconst(float Vgsc,float Vvsc){
    setDiag(Rgsc,Vgsc);
    setDiag(Rvsc,Vvsc);
}

void ScErrStateEKF::setDiag(Matrix& mat, float val){
    for (int i = 1; i < mat.getCols()+1; i++){
            mat(i,i) = val;
    }
}

void ScErrStateEKF::getQhat(float (&res)[4]){
    for (int i = 0; i < 4; i++){
        res[i] = qhat(i+1,1);
    }
}

void ScErrStateEKF::getVihat(float (&res)[3]){
    for (int i = 0; i < 3; i++){
        res[i] = vihat(i+1,1);
    }
}

void ScErrStateEKF::getGyroBias(float (&resVal)[3], float (&resCov)[6]){
    resVal[0] = gyroBias.x;
    resVal[1] = gyroBias.y;
    resVal[2] = gyroBias.z;
    resCov[0] = Phat(4,4);
    resCov[1] = Phat(4,5);
    resCov[2] = Phat(4,6);
    resCov[3] = Phat(5,5);
    resCov[4] = Phat(5,6);
    resCov[5] = Phat(6,6);
    
}

void ScErrStateEKF::getAccBias(float (&resVal)[3], float (&resCov)[6]){
    resVal[0] = accBias.x;
    resVal[1] = accBias.y;
    resVal[2] = accBias.z;
    resCov[0] = Phat(7,7);
    resCov[1] = Phat(7,8);
    resCov[2] = Phat(7,9);
    resCov[3] = Phat(8,8);
    resCov[4] = Phat(8,9);
    resCov[5] = Phat(9,9);
    
}


void ScErrStateEKF::updateAccMeasures(Vector3 acc, Vector3 accref)
{
    acc -= accBias;
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
    Matrix H(3,nState);
    H(1,2) = -2.0f*gvec.z;
    H(1,3) =  2.0f*gvec.y;
    H(2,1) =  2.0f*gvec.z;
    H(2,3) = -2.0f*gvec.x;
    H(3,1) = -2.0f*gvec.y;
    H(3,2) =  2.0f*gvec.x;
    H(1,7) = 1.0f;
    H(2,8) = 1.0f;
    H(3,9) = 1.0f;
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra+Qab);
    Matrix z = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
    Matrix corrVal =  K * (z-H*errState);
    errState = errState + corrVal;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra+Qab)*MatrixMath::Transpose(K);
}

void ScErrStateEKF::updateGyroBiasConstraints(Vector3 gyro)
{
    gyro -= gyroBias;
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix H(2,nState);
    H(1,4) = 1.0f*(dcm(1,1));
    H(1,5) = 1.0f*(dcm(2,1));
    H(1,6) = 1.0f*(dcm(3,1));
    H(2,4) = 1.0f*(dcm(1,2));
    H(2,5) = 1.0f*(dcm(2,2));
    H(2,6) = 1.0f*(dcm(3,2));
    
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rgsc);
    Matrix z(2,1);
    z <<dcm(1,1)*gyro.x+dcm(2,1)*gyro.y+dcm(3,1)*gyro.z<< dcm(1,2)*gyro.x+dcm(2,2)*gyro.y+dcm(3,2)*gyro.z;
    Matrix corrVal =  K * (z-H*errState);
    errState = errState + corrVal;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rgsc)*MatrixMath::Transpose(K);
}


void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref)
{
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
    Matrix H(3,nState);
    H(1,2) = -2.0f*gvec.z;
    H(1,3) =  2.0f*gvec.y;
    H(2,1) =  2.0f*gvec.z;
    H(2,3) = -2.0f*gvec.x;
    H(3,1) = -2.0f*gvec.y;
    H(3,2) =  2.0f*gvec.x;
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra*100.0f);
    Matrix accmat(3,1);
    accmat << acc.x << acc.y << acc.z;
    Matrix gref(3,1);
    gref << 0.0f << 0.0f << accref.z;
    Matrix z = accmat-dcm*gref;
    errState = errState + K * (z-H*errState);
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K);
}

void ScErrStateEKF::updateGPSVelocity(float vi_x, float vi_y, float sinkRate,Vector3 acc, Vector3 accref)
{
    
    Matrix H(3,nState);
    H(1,10)  = 1.0f;
    H(2,11)  = 1.0f;
    H(3,12)  = 1.0f;

    Matrix R(3,3);
    R(1,1) = Rgps(1,1);
    R(2,2) = Rgps(2,2);
    R(3,3) = Rgps(3,3);
    
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    Matrix z(3,1);
    z << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1);
    //z << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1);
    Matrix corrVal =  K * (z-H*errState);
    errState = errState + corrVal;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
}

void ScErrStateEKF::updateSinkRate(float sinkRate,Vector3 acc, Vector3 accref)
{
    acc -= accBias;
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
    Matrix H(4,nState);
    H(1,2) = -2.0f*gvec.z;
    H(1,3) =  2.0f*gvec.y;
    H(2,1) =  2.0f*gvec.z;
    H(2,3) = -2.0f*gvec.x;
    H(3,1) = -2.0f*gvec.y;
    H(3,2) =  2.0f*gvec.x;
    H(1,7) = 1.0f;
    H(2,8) = 1.0f;
    H(3,9) = 1.0f;
    H(4,12)  = 1.0f;
    
    Matrix R(4,4);
    R(1,1) = Ra(1,1)+Qab(1,1);
    R(2,2) = Ra(2,2)+Qab(2,2);
    R(3,3) = Ra(3,3)+Qab(3,3);
    R(4,4) = Rsr(1,1);
    /*
    R(1,1) = 0.0001f;
    R(2,2) = 0.0001f;
    R(3,3) = 0.0001f;
    */
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
    Matrix z(4,1);
    z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << sinkRate - vihat(3,1);
    Matrix corrVal =  K * (z-H*errState);
    errState = errState + corrVal;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
}

void ScErrStateEKF::updateCenteredGyroBiasCorrection(float (&cBg)[3])
{
    
    Matrix RgyroBias(3,3);
    for(int i = 1;i<4;i++){
        RgyroBias(i,i) = 0.1f;
    }
    
    Matrix H(3,nState);
    H(1,4) =  1.0f;
    H(2,5) =  1.0f;
    H(3,6) =  1.0f;
    
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+RgyroBias);
    Matrix z(3,1);
    z <<cBg[0]-gyroBias.x <<cBg[1]-gyroBias.y <<cBg[2]-gyroBias.z;
    Matrix corrVal =  K * (z-H*errState);
    errState = errState + corrVal;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(RgyroBias)*MatrixMath::Transpose(K);
    
}

void ScErrStateEKF::resetBias()
{
    gyroBias.x = gyroBias.x + errState(4,1)*1.0f;
    gyroBias.y = gyroBias.y + errState(5,1)*1.0f;
    gyroBias.z = gyroBias.z + errState(6,1)*1.0f;
    errState(4,1) = 0.0f;
    errState(5,1) = 0.0f;
    errState(6,1) = 0.0f;
    accBias.x = accBias.x + errState(7,1);
    accBias.y = accBias.y + errState(8,1);
    accBias.z = accBias.z + errState(9,1);
    errState(7,1) = 0.0f;
    errState(8,1) = 0.0f;
    errState(9,1) = 0.0f;
}

void ScErrStateEKF::computeAngles(Vector3& rpy,Vector3 rpy_align)
{
    Matrix qerr(4,1);
    qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1);
    //float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qerr),qerr));
    //qerr *= (1.0f/ qnorm);
    Matrix qest = quatmultiply(qhat, qerr);
    float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qest),qest));
    qest *= (1.0f/ qnorm);
    float q0 = qest( 1, 1 );
    float q1 = qest( 2, 1 ); 
    float q2 = qest( 3, 1 ); 
    float q3 = qest( 4, 1 ); 
    rpy.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rpy_align.x;
    rpy.y = asinf(-2.0f * (q1*q3 - q0*q2))-rpy_align.y;
    rpy.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
    
}


void ScErrStateEKF::fuseErr2Nominal()
{
    Matrix qerr(4,1);
    qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1);
    //float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qerr),qerr));
    //qerr *= (1.0f/ qnorm);
    qhat = quatmultiply(qhat, qerr);
    errState(1,1) = 0.0f;
    errState(2,1) = 0.0f;
    errState(3,1) = 0.0f;
    float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
    qhat *= (1.0f/ qnorm);
    
    vihat(1,1) += errState(10,1);
    vihat(2,1) += errState(11,1);
    vihat(3,1) += errState(12,1);
    errState(10,1) = 0.0f;
    errState(11,1) = 0.0f;
    errState(12,1) = 0.0f;
}

Matrix ScErrStateEKF::quatmultiply(Matrix q, Matrix r)
{
    Matrix qout(4,1);
    qout(1,1) = q(1,1)*r(1,1) - q(2,1)*r(2,1) - q(3,1)*r(3,1) - q(4,1)*r(4,1);
    qout(2,1) = q(1,1)*r(2,1) + r(1,1)*q(2,1) + q(3,1)*r(4,1)-q(4,1)*r(3,1);
    qout(3,1) = q(1,1)*r(3,1) + r(1,1)*q(3,1) + q(4,1)*r(2,1)-q(2,1)*r(4,1);
    qout(4,1) = q(1,1)*r(4,1) + r(1,1)*q(4,1) + q(2,1)*r(3,1)-q(3,1)*r(2,1);
    float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qout),qout));
    qout *= (1.0f/ qnorm);
    return qout;
}

void ScErrStateEKF::computeDcm(Matrix& dcm, Matrix quat)
{
    
    float q0 = quat( 1, 1 );
    float q1 = quat( 2, 1 ); 
    float q2 = quat( 3, 1 ); 
    float q3 = quat( 4, 1 ); 
    
    dcm(1,1) = q0*q0 + q1*q1 - q2*q2 - q3*q3;
    dcm(1,2) = 2*(q1*q2 + q0*q3);
    dcm(1,3) = 2*(q1*q3 - q0*q2);
    dcm(2,1) = 2*(q1*q2 - q0*q3);
    dcm(2,2) = q0*q0 - q1*q1 + q2*q2 - q3*q3;
    dcm(2,3) = 2*(q2*q3 + q0*q1);
    dcm(3,1) = 2*(q1*q3 + q0*q2);
    dcm(3,2) = 2*(q2*q3 - q0*q1);
    dcm(3,3) = q0*q0 - q1*q1 - q2*q2 + q3*q3;
    
}

void ScErrStateEKF::defineQhat(Vector3 align){
        float cos_z_2 = cosf(0.5f*align.z);
        float cos_y_2 = cosf(0.5f*align.y);
        float cos_x_2 = cosf(0.5f*align.x);
 
        float sin_z_2 = sinf(0.5f*align.z);
        float sin_y_2 = sinf(0.5f*align.y);
        float sin_x_2 = sinf(0.5f*align.x);
 
        // and now compute quaternion
        qhat(1,1)   = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
        qhat(2,1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
        qhat(3,1) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
        qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;    
}

void ScErrStateEKF::computeVb(Vector3& vb)
{
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix vbmat = dcm*vihat;
    vb.x = vbmat(1,1);
    vb.y = vbmat(2,1);
    vb.z = vbmat(3,1);
    
}



Vector3 ScErrStateEKF::calcDynAcc(Vector3 acc, Vector3 accref)
{
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    dcm = MatrixMath::Transpose(dcm);
    float _x, _y, _z;
    _x = acc.x-(dcm( 1, 1 )*accref.x+dcm( 1, 2 )*accref.y+dcm( 1, 3 )*accref.z);
    _y = acc.y-(dcm( 2, 1 )*accref.x+dcm( 2, 2 )*accref.y+dcm( 2, 3 )*accref.z);
    _z = acc.z-(dcm( 3, 1 )*accref.x+dcm( 3, 2 )*accref.y+dcm( 3, 3 )*accref.z);
    return Vector3(_x, _y, _z);
}


bool ScErrStateEKF::determinDynStatus(Vector3 acc, Vector3 accref)
{
        histffunc[histffuncindex] = acc.Norm()*acc.Norm()-accref.Norm()*accref.Norm()-3.0f*sigma2a;
        if(histffuncindex<9){
            histffuncindex += 1;
        }else{
            histffuncindex =0;
        }
        aveffunc = 0;
        for(int i = 1;i<10;i++){
            aveffunc += 1.0f/10.0f*histffunc[i]; 
        }
        sigma2f = 1.0f/10.0f*(6.0f*sigma2a*sigma2a+4.0f*accref.Norm()*accref.Norm()*sigma2a);
        
        
        dynacc = calcDynAcc(acc,accref);
        bool dynCase = true;
        if((dynacc.Norm()<0.005f) && (abs(acc.Norm()-accref.Norm())<0.005f)){dynCase = false;}
        if(aveffunc<sqrt(sigma2f/0.99f) && abs(acc.Norm()-accref.Norm())<0.001f){dynCase = false;}
        return dynCase;
        
}

/*
void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt)
{
    gyro -= gyroBias;
    acc -= accBias;
    Matrix A(4,4);
    A <<  0.0f        << -0.5f*gyro.x <<-0.5f*gyro.y <<-0.5f*gyro.z
      <<  0.5f*gyro.x <<  0.0f        << 0.5f*gyro.z <<-0.5f*gyro.y
      <<  0.5f*gyro.y << -0.5f*gyro.z << 0.0f        << 0.5f*gyro.x
      <<  0.5f*gyro.z <<  0.5f*gyro.y <<-0.5f*gyro.x << 0.0f ;
        
    Matrix phi = MatrixMath::Eye(4) + A * att_dt + (0.5f*att_dt*att_dt) * A * A;
    qhat = phi * qhat;
    float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
    qhat *= (1.0f/ qnorm);
    
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    vihat += (MatrixMath::Transpose(dcm)*MatrixMath::Vector2mat(acc)-MatrixMath::Vector2mat(accref))*att_dt*9.8f;
}

void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt)
{
    gyro -= gyroBias;
    acc -= accBias;
    Matrix A(nState,nState);
    A(1,2) =  gyro.z;
    A(1,3) = -gyro.y;
    A(2,1) = -gyro.z;
    A(2,3) =  gyro.x;
    A(3,1) =  gyro.y;
    A(3,2) = -gyro.x;
    A(1,4) =  -0.5f;
    A(2,5) =  -0.5f;
    A(3,6) =  -0.5f;
    
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix qeterm = -2.0f*9.8f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(acc.x,acc.y,acc.z);
    Matrix baterm = -9.8f*MatrixMath::Transpose(dcm);
    for (int i = 1; i < 4; i++){
        for (int j = 1; i < 4; i++){
            A(i+9,j) = qeterm(i,j);
            A(i+9,j+6) = baterm(i,j);
        }
    }
    
    Matrix phi = MatrixMath::Eye(nState) + A * att_dt + (0.5f*att_dt*att_dt) * A * A;
    Matrix Qd = Q * att_dt + 0.5f*A*Q * att_dt * att_dt+ 0.5f*Q*MatrixMath::Transpose(A) * att_dt * att_dt;
    errState = phi * errState;
    Phat = phi*Phat*MatrixMath::Transpose(phi)+Qd;
}

void ScErrStateEKF::updateVelocityConstraints()
{
    
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix H(2,nState);
    
    Matrix nomVb = dcm*vihat;
    Matrix qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
    for(int j = 1;j<4;j++){
        H(1,j) = -qeterm(1,j);
        H(2,j) = -qeterm(3,j);
        //H(3,j) = -qeterm(3,j);
        H(1,9+j) = -dcm(1,j);
        H(2,9+j) = -dcm(3,j);
        //H(3,9+j) = -dcm(3,j);
    }
    
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc);
    Matrix z(2,1);
    z << nomVb(1,1)  << nomVb(3,1) ;
    Matrix corrVal =  K * (z-H*errState);
    errState = errState + corrVal;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K);
    
}

void ScErrStateEKF::updateConstraints(Vector3 gyro)
{
    
    gyro -=gyroBias;
    
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    
    Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
    Matrix H(4,nState);
    for(int i = 1; i<3; i++){
        for(int j = 1;j<4;j++){
            H(i,j) = qeterm(i,j);
        }
    }
    H(1,4) = 1.0f*(dcm(1,1));
    H(1,5) = 1.0f*(dcm(2,1));
    H(1,6) = 1.0f*(dcm(3,1));
    H(2,4) = 1.0f*(dcm(1,2));
    H(2,5) = 1.0f*(dcm(2,2));
    H(2,6) = 1.0f*(dcm(3,2));
    
    Matrix nomVb = dcm*vihat;
    qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
    for(int j = 1;j<4;j++){
        H(3,j) = -qeterm(1,j);
        H(4,j) = -qeterm(3,j);
        //H(3,j) = -qeterm(3,j);
        H(3,9+j) = -dcm(1,j);
        H(4,9+j) = -dcm(3,j);
        //H(3,9+j) = -dcm(3,j);
    }
    
    Matrix R(4,4);
    R(1,1) = Rgsc(1,1);
    R(2,2) = Rgsc(2,1);
    R(3,3) = Rvsc(1,1);
    R(4,4) = Rvsc(2,1);
    //R(5,5) = Rvsc(3,1);
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    Matrix z(4,1);
    z <<dcm(1,1)*gyro.x+dcm(2,1)*gyro.y+dcm(3,1)*gyro.z<< dcm(1,2)*gyro.x+dcm(2,2)*gyro.y+dcm(3,2)*gyro.z << nomVb(1,1)  << nomVb(3,1) ;
    Matrix corrVal =  K * (z-H*errState);
    errState = errState + corrVal;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
}  

void ScErrStateEKF::computeVb(Vector3& vb)
{
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix vbmat = dcm*vihat;
    vb.x = vbmat(1,1);
    vb.y = vbmat(2,1);
    vb.z = vbmat(3,1);
    
}

void ScErrStateEKF::updateMagMeasures(Vector3 mag)
{
    //mag = mag/mag.Norm();
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    
    Matrix magvec(3,1);
    magvec(1,1) = mag.x;
    magvec(2,1) = mag.y;
    magvec(3,1) = mag.z;
    
    Matrix magnedvec = MatrixMath::Transpose(dcm)*magvec;
    Matrix magrefmod(3,1);
    magrefmod(1,1) = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1));
    magrefmod(2,1) = 0.0f;
    magrefmod(3,1) = magnedvec(3,1);
    
    Matrix mvec = dcm*magrefmod ;
    Matrix H(3,nState);
    H(1,2) = -2.0f*magvec(3,1);
    H(1,3) =  2.0f*magvec(2,1);
    H(2,1) =  2.0f*magvec(3,1);
    H(2,3) = -2.0f*magvec(1,1);
    H(3,1) = -2.0f*magvec(2,1);
    H(3,2) =  2.0f*magvec(1,1);
    Matrix Pm(nState,nState);
    for(int i = 1; i<4; i++){
        for(int j = 1;j<4;j++){
            Pm(i,j) = Phat(i,j);
        }
    }
    Matrix r3(3,1);
    r3 <<  dcm(1,3)<<  dcm(2,3) <<  dcm(3,3);
    Matrix kpart  = r3*MatrixMath::Transpose(r3);
    Matrix Kmod(nState,nState);
    for(int i = 1; i<4; i++){
        for(int j = 1;j<4;j++){
            Kmod(i,j) = kpart(i,j);
        }
    }
    
    Matrix K = Kmod*(Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+Rm);
    Matrix z = magvec-mvec;
    errState = errState + K * (z-H*errState);
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rm)*MatrixMath::Transpose(K);
}

void ScErrStateEKF::updateVelocityConstraints()
{
    
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix H(2,nState);
    
    Matrix nomVb = dcm*vihat;
    Matrix qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
    for(int j = 1;j<4;j++){
        H(1,j) = -qeterm(1,j);
        H(2,j) = -qeterm(3,j);
        //H(3,j) = -qeterm(3,j);
        H(1,9+j) = -dcm(1,j);
        H(2,9+j) = -dcm(3,j);
        //H(3,9+j) = -dcm(3,j);
    }
    
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc);
    Matrix z(2,1);
    z << nomVb(1,1)  << nomVb(3,1) ;
    Matrix corrVal =  K * (z-H*errState);
    errState = errState + corrVal;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K);
    
}
*/