File content as of revision 70:d12e46fdc2f0:

#include "solaESKF.hpp"



solaESKF::solaESKF()
    :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),magBias(3,1),errState(16,1),Phat(16,16),Q(16,16)
{
    pihat << 0.0f << 0.0f << 0.0f;
    vihat << 0.0f << 0.0f << 0.0f;
    qhat << 1.0f << 0.0f << 0.0f << 0.0f;
    accBias << 0.0f << 0.0f << 0.0f;
    gyroBias << 0.0f << 0.0f << 0.0f;
    gravity << 0.0f << 0.0f << 9.8f;
    magBias << 0.0f << 0.0f << 0.0f;
    magRadius = 0.0f;

    nState = errState.getRows();

    for (int i = 1; i < nState+1; i++){
        errState(i,1) = 0.0f;
        for (int j = 1; j < nState+1; j++){
            Phat(i,j) = 0.0f;
            Q(i,j) = 0.0f;
        }
    }

    setBlockDiag(Phat,0.1f,1,3);//position
    setBlockDiag(Phat,0.1f,4,6);//velocity
    setBlockDiag(Phat,0.1f,7,9);//angle error
    setBlockDiag(Phat,0.1f,10,12);//acc bias
    setBlockDiag(Phat,0.1f,13,15);//gyro bias
    setBlockDiag(Phat,0.00000001f,16,16);//gravity
    setBlockDiag(Q,0.00025f,4,6);//velocity
    setBlockDiag(Q,0.005f/57.0f,7,9);//angle error
    setBlockDiag(Q,0.001f,10,12);//acc bias
    setBlockDiag(Q,0.001f,13,15);//gyro bias//positionとgravityはQ項なし
    
}


void solaESKF::updateNominal(Matrix acc, Matrix gyro,float att_dt)
{
    Matrix gyrom = gyro  - gyroBias;
    Matrix accm = acc - accBias;
    
    Matrix qint(4,1);
    qint << 1.0f << 0.5f*gyrom(1,1)*att_dt << 0.5f*gyrom(2,1)*att_dt << 0.5f*gyrom(3,1)*att_dt; 
    qhat = quatmultiply(qhat,qint);
    float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
    qhat *= (1.0f/ qnorm);
    
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix accned = dcm*accm+gravity;
    vihat += accned*att_dt;
    
    pihat += vihat*att_dt+0.5f*accned*att_dt*att_dt;
}

void solaESKF::updateErrState(Matrix acc,Matrix gyro,float att_dt)
{
    Matrix gyrom = gyro  - gyroBias;
    Matrix accm = acc - accBias;
    
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1));
    
    Matrix A(nState,nState);
    //position
    A(1,4) =  1.0f;
    A(2,5) =  1.0f;
    A(3,6) =  1.0f;
    
    //velocity
    for (int i = 1; i < 4; i++){
        for (int j = 1; j < 4; j++){
            A(i+3,j+6) = a2v(i,j);
            A(i+3,j+9) = -dcm(i,j);

        }
    }
    A(6,16) =  1.0f;
    
    //angulat error
    A(7,8) =  gyrom(3,1);
    A(7,9) = -gyrom(2,1);
    A(8,7) = -gyrom(3,1);
    A(8,9) =  gyrom(1,1);
    A(9,7) =  gyrom(2,1);
    A(9,8) = -gyrom(1,1);
    A(7,13) =  -1.0f;
    A(8,14) =  -1.0f;
    A(9,15) =  -1.0f;
    

    Matrix Fx = MatrixMath::Eye(nState) + A * att_dt;// + (0.5f*att_dt*att_dt) * A * A;
    for (int i = 1; i < nState+1; i++){
        if(i>3 && i<10){
            Q(i,i) *=att_dt;
        }else if(i>9 && i<16){
            Q(i,i) *= att_dt*att_dt;
        }else if(i>16 && i<nState+1){
            Q(i,i) *=att_dt*att_dt;
        }else{
            Q(i,i) = 0.0f;
        }      
    }
    errState = Fx * errState;
    Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Q;
}

void solaESKF::updateAcc(Matrix acc,Matrix R)
{
    Matrix accm = acc - accBias;
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix tdcm = MatrixMath::Transpose(dcm);
    Matrix tdcm_g = tdcm*gravity;
    Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
    
    Matrix H(3,nState);
    for (int i = 1; i < 4; i++){
        for (int j = 1; j < 4; j++){
            H(i,j+6) =  rotgrav(i,j);
        }
        H(i,16) = tdcm(i,3);
    }

    H(1,10) =  -1.0f;
    H(2,11) =  -1.0f;
    H(3,12) =  -1.0f;
    
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    Matrix zacc = -accm-tdcm*gravity;
    Matrix z(3,1);
    z << zacc(1,1) << zacc(2,1) << zacc(3,1);
    errState =  K * z;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
    
    fuseErr2Nominal();
}
void solaESKF::updateMag(Matrix mag,Matrix R)
{
    Matrix magm = mag-magBias;
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix tdcm = MatrixMath::Transpose(dcm);
    Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
    
    Matrix H(2,nState);
    int nH = H.getRows();
    
    Matrix magned = dcm*magm;
    float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
    float hz = sqrt(magned(3,1)*magned(3,1));
    float a = sqrt(magm(1,1)*magm(1,1)+magm(2,1)*magm(2,1)+magm(3,1)*magm(3,1));

    H(1,3+6) =  rotmag(1,3)-(rotmag(1,3)+rotmag(2,3))/hx;
    H(2,3+6) =  rotmag(2,3);
    for(int j = 1; j < 4; j++){
        //H(3,j+6) =  rotmag(3,j)-(rotmag(3,j))/hz;
        H(1,j+16) =  -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
        H(2,j+16) =  -dcm(2,j);
        //H(3,j+16) =  -dcm(3,j)+(dcm(3,j))/hz;
    }
    //H(3,20) = 1.0f;
    //H(3,17) = magm(1,1)/a;
    //H(3,18) = magm(2,1)/a;
    //H(3,19) = magm(3,1)/a;
    
    Matrix z(nH,1);
    z << -(magned(1,1) - hx) << -magned(2,1);
    //twelite.printf("%f %f %f %f\r\n",-(magned(1,1) - hx),-magned(2,1),-(magned(3,1) - hz),-(magRadius-a));
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    
    /*
    Matrix r3(3,1);
    r3 <<  tdcm(1,3)<<  tdcm(2,3) <<  tdcm(3,3);
    Matrix kpart  = r3*MatrixMath::Transpose(r3);
    Matrix Pm(nState,nState);
    for(int i = 7; i<10; i++){
        for(int j = 7;j<10;j++){
            Pm(i,j) = Phat(i,j);
        }
    }
    for(int i = 17; i<nState+1; i++){
        for(int j = 17;j<nState+1;j++){
            Pm(i,j) = Phat(i,j);
        }
    }
    
    Matrix K = (Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+R);
    Matrix Kmod(3,nH);
    for(int i = 1; i<4; i++){
        for(int j = 1;j<nH+1;j++){
            Kmod(i,j) = K(i+6,j);
        }
    }
    Kmod = kpart*Kmod;
    for(int i = 1; i<nState+1; i++){
        for(int j = 1;j<nH;j++){
            if(i>6 && i<10){
                K(i,j) = Kmod(i-6,j);
            }else if(i<17){
                K(i,j) = 0.0f;
            }
        }
    }
    */
    errState =  K * z;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
    
    fuseErr2Nominal();
}

void solaESKF::updateHeading(Matrix mag,Matrix R)
{
    float q0 = qhat(1,1);
    float q1 = qhat(2,1);
    float q2 = qhat(3,1);
    float q3 = qhat(4,1);
    
    float d0 = (-q3*q3-q2*q2+q1*q1+q0*q0);
    float q0q3q1q2 = (q0*q3+q1*q2);
    float h1lower = 2.0f*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
    
    float d1 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
    float d2 = d0*d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
    float d3 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
    
    
    
    Matrix Hh(2,4);
    Hh(1,1) = -(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
    Hh(1,2) = -(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
    Hh(1,3) = -(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
    Hh(1,4) = -(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
    
    Hh(2,1) = 2.0f*q3/d1-4.0f*q0*q0q3q1q2/d2-q0q3q1q2*(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
    Hh(2,2) = 2.0f*q2/d1-4.0f*q1*q0q3q1q2/d2-q0q3q1q2*(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
    Hh(2,3) = 2.0f*q1/d1-4.0f*q2*q0q3q1q2/d2-q0q3q1q2*(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
    Hh(2,4) = 2.0f*q0/d1-4.0f*q3*q0q3q1q2/d2-q0q3q1q2*(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
    
    Matrix Hdq(4,3);
    Hdq  << -0.5f*q1 << -0.5f*q2 << -0.5f*q3
         << 0.5f*q0 << -0.5f*q3 << 0.5f*q2
         << 0.5f*q3 << 0.5f*q0 << -0.5f*q1
         << -0.5f*q2 << 0.5f*q1 << 0.5f*q0; 
    
    Matrix Hpart = Hh*Hdq;
    Matrix H(2,nState);
    for(int j=1;j<4;j++){
        H(1,j+6) = Hpart(1,j);
        H(2,j+6) = Hpart(2,j);
    }
    
    Matrix z(2,1);
    float a = 0.5f;
    float gamma = 2.0f*q0q3q1q2/d0;
    z << 1.0f/sqrt(a*a+1.0f)-1.0f/sqrt(gamma*gamma+1.0f) << a/sqrt(a*a+1.0f)-gamma/sqrt(gamma*gamma+1.0f);
    
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    errState =  K * z;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
    
    fuseErr2Nominal();
}
void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
{
    Matrix accm = acc - accBias;
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix tdcm = MatrixMath::Transpose(dcm);
    Matrix tdcm_g = tdcm*gravity;
    Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
    Matrix rotmag = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
    
    Matrix H(6,nState);
    for (int i = 1; i < 3; i++){
        for (int j = 1; j < 4; j++){
            H(i,j+6) =  rotgrav(i,j);
        }
        H(i,16) = tdcm(i,3);
    }

    H(1,10) =  -1.0f;
    H(2,11) =  -1.0f;
    H(3,12) =  -1.0f;
    
    Matrix magned = dcm*mag;
    float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
    float hz = sqrt(magned(3,1)*magned(3,1));
    
    for(int j = 3; j < 4; j++){
        H(4,j+6) =  rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx;
        //H(4,j+16) =  -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
        H(5,j+6) =  rotmag(2,j);
        //H(5,j+16) =  -dcm(2,j);
        H(6,j+6) =  rotmag(3,j)-(rotmag(3,j))/hz;
    }
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    /*
    Matrix r3(3,1);
    r3 <<  tdcm(1,3)<<  tdcm(2,3) <<  tdcm(3,3);
    Matrix kpart  = r3*MatrixMath::Transpose(r3);
    Matrix Kmod(3,3);
    for(int i = 1; i<4; i++){
        for(int j = 1;j<4;j++){
            Kmod(i,j) = K(i+6,j+3);
        }
    }
    Kmod = kpart*Kmod;
    */
    for(int i = 1; i<nState+1; i++){
        for(int j = 4;j<7;j++){
            if(i>6 && i<10){
                //K(i,j) = Kmod(i-6,j-3);
            }else{
                K(i,j) = 0.0f;
            }
        }
    }
    
    Matrix zacc = -accm-tdcm*gravity;
    Matrix z(6,1);
    z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(magned(1,1) - hx) << -magned(2,1) << -(magned(3,1) - hz);
    //twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
    errState =  K * z;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
    
    fuseErr2Nominal();
}


void solaESKF::updateOtherConstraints(Matrix mag,float palt,Matrix R)
{
    Matrix magm = mag - magBias;
    float a = sqrt(magm(1,1)*magm(1,1)+magm(2,1)*magm(2,1)+magm(3,1)*magm(3,1));
    Matrix H(2,nState);
    H(1,20) = -1.0f;
    H(1,17) = -magm(1,1)/a;
    H(1,18) = -magm(2,1)/a;
    H(1,19) = -magm(3,1)/a;
    H(2,3) = 1.0f;

    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    
    Matrix z(2,1);
    z << -(a-magRadius) << palt-pihat(3,1);
    //twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
    errState =  K * z;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
    
    fuseErr2Nominal();
}


void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R)
{
    Matrix H(5,nState);
    H(1,1)  = 1.0f;
    H(2,2)  = 1.0f;
    H(3,3)  = 1.0f;
    H(4,4)  = 1.0f;
    H(5,5)  = 1.0f;
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    Matrix z(5,1);
    z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1) << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1);
    errState =  K * z;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
    fuseErr2Nominal();
}


Matrix solaESKF::computeAngles()
{
    
    Matrix euler(3,1);
    euler(1,1) = atan2f(qhat(1,1)*qhat(2,1) + qhat(3,1)*qhat(4,1), 0.5f - qhat(2,1)*qhat(2,1) - qhat(3,1)*qhat(3,1));
    euler(2,1) = asinf(-2.0f * (qhat(2,1)*qhat(4,1) - qhat(1,1)*qhat(3,1)));
    euler(3,1) = atan2f(qhat(2,1)*qhat(3,1) + qhat(1,1)*qhat(4,1), 0.5f - qhat(3,1)*qhat(3,1) - qhat(4,1)*qhat(4,1));
    return euler;
}


void solaESKF::fuseErr2Nominal()
{
    //position
    pihat(1,1) += errState(1,1);
    pihat(2,1) += errState(2,1);
    pihat(3,1) += errState(3,1);
    
    //velocity
    vihat(1,1) += errState(4,1);
    vihat(2,1) += errState(5,1);
    vihat(3,1) += errState(6,1);
    
    //angle error
    Matrix qerr(4,1);
    qerr << 1.0f << 0.5f*errState(7,1) << 0.5f*errState(8,1) << 0.5f*errState(9,1);
    qhat = quatmultiply(qhat, qerr);
    float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
    qhat *= (1.0f/ qnorm);
    
    //acc bias
    accBias(1,1) += errState(10,1);
    accBias(2,1) += errState(11,1);
    accBias(3,1) += errState(12,1);
    
    //gyro bias
    gyroBias(1,1) += errState(13,1);
    gyroBias(2,1) += errState(14,1);
    gyroBias(3,1) += errState(15,1);

    //gravity bias
    gravity(1,1) = 0.0f;
    gravity(2,1) = 0.0f;
    gravity(3,1) += errState(16,1);

    
    
    for (int i = 1; i < nState+1; i++){
        errState(i,1) = 0.0f;
    }

}

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

void solaESKF::computeDcm(Matrix& dcm, Matrix quat)
{
    
    dcm(1,1) = quat(1,1)*quat(1,1) + quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1);
    dcm(1,2) = 2.0f*(quat(2,1)*quat(3,1) - quat(1,1)*quat(4,1));
    dcm(1,3) = 2.0f*(quat(2,1)*quat(4,1) + quat(1,1)*quat(3,1));
    dcm(2,1) = 2.0f*(quat(2,1)*quat(3,1) + quat(1,1)*quat(4,1));
    dcm(2,2) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) + quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1);
    dcm(2,3) = 2.0f*(quat(3,1)*quat(4,1) - quat(1,1)*quat(2,1));
    dcm(3,1) = 2.0f*(quat(2,1)*quat(4,1) - quat(1,1)*quat(3,1));
    dcm(3,2) = 2.0f*(quat(3,1)*quat(4,1) + quat(1,1)*quat(2,1));
    dcm(3,3) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) + quat(4,1)*quat(4,1);
    
}

void solaESKF::setQhat(float ex,float ey,float ez)
{
        float cos_z_2 = cosf(0.5f*ez);
        float cos_y_2 = cosf(0.5f*ey);
        float cos_x_2 = cosf(0.5f*ex);
 
        float sin_z_2 = sinf(0.5f*ez);
        float sin_y_2 = sinf(0.5f*ey);
        float sin_x_2 = sinf(0.5f*ex);
 
        // 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 solaESKF::setGravity(float gx,float gy,float gz)
{
    gravity(1,1) = gx;
    gravity(2,1) = gy;
    gravity(3,1) = gz;
}

Matrix solaESKF::getPihat()
{
    return pihat;
}
Matrix solaESKF::getVihat()
{
    return vihat;
}
Matrix solaESKF::getQhat()
{
    return qhat;
}
Matrix solaESKF::getAccBias()
{
    return accBias;
}
Matrix solaESKF::getGyroBias()
{
    return gyroBias;
}
Matrix solaESKF::getGravity()
{
    return gravity;
}
Matrix solaESKF::getMagBias()
{
    return magBias;
}
float solaESKF::getMagRadius()
{
    return magRadius;
}
Matrix solaESKF::getErrState()
{
    return errState;
}

void solaESKF::setPhatPosition(float val)
{
    setBlockDiag(Phat,val,1,3);
}
void solaESKF::setPhatVelocity(float val)
{
    setBlockDiag(Phat,val,4,6);
}
void solaESKF::setPhatAngleError(float val)
{
    setBlockDiag(Phat,val,7,9);
}
void solaESKF::setPhatAccBias(float val)
{
    setBlockDiag(Phat,val,10,12);
}
void solaESKF::setPhatGyroBias(float val)
{
    setBlockDiag(Phat,val,13,15);
}
void solaESKF::setPhatGravity(float val)
{
    setBlockDiag(Phat,val,16,16);
}
void solaESKF::setPhatMagBias(float val)
{
    setBlockDiag(Phat,val,17,19);
}
void solaESKF::setPhatMagRadius(float val)
{
    setBlockDiag(Phat,val,20,20);
}


void solaESKF::setQVelocity(float val)
{
    setBlockDiag(Q,val,4,6);
}
void solaESKF::setQAngleError(float val)
{
    setBlockDiag(Q,val,7,9);
}
void solaESKF::setQAccBias(float val)
{
    setBlockDiag(Q,val,10,12);
}
void solaESKF::setQGyroBias(float val)
{
    setBlockDiag(Q,val,13,15);
}
void solaESKF::setQMagBias(float val)
{
    setBlockDiag(Q,val,17,19);
}
void solaESKF::setQMagRadius(float val)
{
    setBlockDiag(Q,val,20,20);
}


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

void solaESKF::setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex){
    for (int i = startIndex; i < endIndex+1; i++){
            mat(i,i) = val;
    }
}

void solaESKF::setPihat(float pi_x, float pi_y)
{
    pihat(1,1) = pi_x;
    pihat(2,1) = pi_y;
    //pihat(3,1) = pi_z;
}
/*
void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R)
{
    Matrix accm = acc - accBias;
    Matrix tdcm(3,3);
    computeDcm(tdcm, qhat);
    tdcm = MatrixMath::Transpose(tdcm);
    Matrix tdcm_g = tdcm*gravity;
    Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
    
    Matrix H(4,nState);
    for (int i = 1; i < 4; i++){
        for (int j = 1; j < 4; j++){
            H(i,j+6) =  a2v(i,j);
            H(i,j+15) = tdcm(i,j);
        }
    }
    H(1,10) =  -1.0f;
    H(2,11) =  -1.0f;
    H(3,12) =  -1.0f;
    H(4,3) = 1.0f;
    
    //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
    //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    Matrix zacc = -accm-tdcm*gravity;
    Matrix z(4,1);
    z << zacc(1,1) << zacc(2,1) << zacc(3,1) << palt - pihat(3,1);
    errState =  K * z;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
    //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
    //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
    fuseErr2Nominal();
}

void solaESKF::updateGyroConstraints(Matrix gyro,Matrix R)
{
    Matrix gyrom = gyro - gyroBias;
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix a2v = dcm*MatrixMath::Matrixcross(gyrom(1,1),gyrom(2,1),gyrom(3,1));
    
    Matrix H(2,nState);
    for (int i = 1; i < 3; i++){
        for (int j = 1; j < 4; j++){
            H(i,j+6) =  a2v(i,j);
            H(i,j+12) = -dcm(i,j);
        }
    }
    //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
    //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(2))*MatrixMath::Transpose(A));
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    
    Matrix z3 = -dcm*gyrom;
    Matrix z(2,1);
    z << z3(1,1) << z3(2,1);
    errState =  K * z;
    //Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
    //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
    fuseErr2Nominal();
}
void solaESKF::updateMag(Matrix mag, Matrix R)
{
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
    
    Matrix H(2,nState);
    for (int i = 1; i < 3; i++){
        for (int j = 1; j < 4; j++){
            H(i,j+6) =  a2v(i,j);
        }
    }
    H(1,19) = 1.0f;
    //H(3,20) = 1.0f;
    
    //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
    //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    Matrix zmag = -dcm*mag-magField;
    Matrix z(2,1);
    z << zmag(1,1) << zmag(2,1);
    errState =  K * z;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
    //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
    //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
    fuseErr2Nominal();
}
void solaESKF::updateMag(Matrix mag,float palt, Matrix R)
{
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
    
    Matrix H(3,nState);
    for (int i = 1; i < 3; i++){
        for (int j = 1; j < 4; j++){
            H(i,j+6) =  a2v(i,j);
        }
    }
    H(1,19) = 1.0f;
    H(3,3)  = 1.0f;
    //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
    //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    Matrix zmag = -dcm*mag-magField;
    Matrix z(3,1);
    z << zmag(1,1) << zmag(2,1) <<  palt - pihat(3,1);
    errState =  K * z;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
    //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
    //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
    
    fuseErr2Nominal();
}
void solaESKF::updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R)
{
    
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
    
    
    Matrix H(3,nState);
    H(1,4)  = 1.0f;
    H(2,5)  = 1.0f;
    
    for (int j = 1; j < 4; j++){
        H(3,j+6) =  a2v(2,j);
    }
    //H(3,19) = 1.0f;
    
    Matrix zmag = -dcm*mag-magField;
    
    //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
    //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    Matrix z(3,1);
    z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << zmag(2,1);
    errState =  K * z;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
    //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
    //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
    fuseErr2Nominal();
}

void solaESKF::updateGPSPosition(Matrix posgps,float palt,Matrix R)
{
    Matrix H(3,nState);
    H(1,1)  = 1.0f;
    H(2,2)  = 1.0f;
    H(3,3)  = 1.0f;
    
    //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
    //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    Matrix z(3,1);
    z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1);
    errState =  K * z;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
    //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
    //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
    fuseErr2Nominal();
}

void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
{
    Matrix accm = acc - accBias;
    Matrix magm = mag - magBias;
    Matrix dcm(3,3);
    computeDcm(dcm, qhat);
    Matrix tdcm = MatrixMath::Transpose(dcm);
    Matrix tdcm_g = tdcm*gravity;
    Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
    Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
    
    Matrix H(5,nState);
    for (int i = 1; i < 4; i++){
        for (int j = 1; j < 4; j++){
            H(i,j+6) =  rotgrav(i,j);
        }
        H(i,16) = tdcm(i,3);
    }

    H(1,10) =  -1.0f;
    H(2,11) =  -1.0f;
    H(3,12) =  -1.0f;
    
    Matrix magned = dcm*magm;
    float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
    
    for(int j = 1; j < 4; j++){
        H(4,j+6) =  rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx;
        H(4,j+16) =  -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
        H(5,j+6) =  rotmag(2,j);
        H(5,j+16) =  -dcm(2,j);
    }
    
    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    Matrix zacc = -accm-tdcm*gravity;
    Matrix zmag = dcm*magm;
    Matrix z(5,1);
    z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(zmag(1,1) - hx) << -zmag(2,1);
    twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
    errState =  K * z;
    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
    
    fuseErr2Nominal();
}
*/