File content as of revision 44:7d82e63b6a86:

#include "solaESKF.hpp"
#include "Matrix.h"
#include "MatrixMath.h"
#include <cmath>


solaESKF::solaESKF()
{ 
    //nominal state
    pihat(3,1);
    vihat(3,1);
    qhat(4,1);
    accBias(3,1);
    gyroBias(3,1);
    gravity(3,1);
    
    
    errState(18,1);
    nState = errState.getRows();
    Phat(nState,nState);
    Q(nState,nState);
    
    setDiag(Phat,0.1f); 
    setDiag(Q,0.01f);
    setBlockDiag(Q,0.0f,1,3);
    setBlockDiag(Q,0.00001f,16,18);
    
    
}

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 << gyrom(1,1)*att_dt << gyrom(2,1)*att_dt << 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*acc+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 Fx(nState,nState);
    //position
    Fx(1,1) =  1.0f;
    Fx(2,2) =  1.0f;
    Fx(3,3) =  1.0f;
    Fx(1,4) =  1.0f*att_dt;
    Fx(2,5) =  1.0f*att_dt;
    Fx(3,6) =  1.0f*att_dt;
    
    //velocity
    Fx(4,4) =  1.0f;
    Fx(5,5) =  1.0f;
    Fx(6,6) =  1.0f;
    Matrix a2v = -dcm*MatrixMath::Matrixcross(acc(1,1),acc(2,1),acc(3,1))*att_dt;
    for (int i = 1; i < 4; i++){
        for (int j = 1; j < 4; j++){
            Fx(i+3,j+6) = a2v(i,j);
            Fx(i+3,j+9) = -dcm(i,j)*att_dt;
        }
    }
    Fx(4,16) =  1.0f*att_dt;
    Fx(5,17) =  1.0f*att_dt;
    Fx(6,18) =  1.0f*att_dt;
    
    //angulat error
    Fx(7,8) =  gyrom(3,1)*att_dt;
    Fx(7,9) = -gyrom(2,1)*att_dt;
    Fx(8,7) = -gyrom(3,1)*att_dt;
    Fx(8,9) =  gyrom(1,1)*att_dt;
    Fx(9,7) =  gyrom(2,1)*att_dt;
    Fx(9,8) = -gyrom(1,1)*att_dt;
    Fx(7,13) =  -1.0f*att_dt;
    Fx(8,14) =  -1.0f*att_dt;
    Fx(9,15) =  -1.0f*att_dt;
    
    //acc bias
    Fx(10,10) =  1.0f;
    Fx(11,11) =  1.0f;
    Fx(12,12) =  1.0f;
    
    //gyro bias
    Fx(13,13) =  1.0f;
    Fx(14,14) =  1.0f;
    Fx(15,15) =  1.0f;
    
    //gravity bias
    Fx(16,16) =  1.0f;
    Fx(17,17) =  1.0f;
    Fx(18,18) =  1.0f;
    
    errState = Fx * errState;
    Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Q*att_dt*att_dt;
}

/*
void solaESKF::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);
}
*/
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(1,7) << 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) += errState(16,1);
    gravity(2,1) += errState(17,1);
    gravity(3,1) += errState(18,1);

}

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;
}



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 endIndez){
    for (int i = startIndex; i < endIndex+1; i++){
            mat(i,i) = val;
    }
}