Diff: ScErrStateEKF.cpp

Revision:

19:3fae66745363

Child:

20:37d3c3ee36e9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ScErrStateEKF.cpp	Fri Jul 23 05:55:28 2021 +0000
@@ -0,0 +1,392 @@
+#include "ScErrStateEKF.hpp"
+#include "Matrix.h"
+#include "MatrixMath.h"
+#include <cmath>
+#include "Vector3.hpp"
+
+
+using namespace std;
+
+ScErrStateEKF::ScErrStateEKF()
+    :qhat(4,1), errState(12,1), Phat(12,12), Q(12,12), Ra(5,5), Rm(3,3), Qab(5,5),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f)
+{ 
+    qhat << 1.0f << 0.0f << 0.0f << 0.0f;
+    errState << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f;
+    
+    Phat(1,1) = 0.1f;
+    Phat(2,2) = 0.1f;
+    Phat(3,3) = 0.1f;
+    Phat(4,4) = 0.1f;
+    Phat(5,5) = 0.1f;
+    Phat(6,6) = 0.1f;
+    Phat(7,7) = 0.1f;
+    Phat(8,8) = 0.1f;
+    Phat(9,9) = 0.1f;
+    Phat(10,10) = 0.1f;
+    Phat(11,11) = 0.1f;
+    Phat(12,12) = 0.1f;
+     
+    Q(1,1) = 0.000005f*0.25f;
+    Q(2,2) = 0.000005f*0.25f; 
+    Q(3,3) = 0.000005f*0.25f;
+    Q(4,4) = 0.0005f;
+    Q(5,5) = 0.0005f; 
+    Q(6,6) = 0.0005f;
+    Q(7,7) = 100.0f;
+    Q(8,8) = 100.0f;
+    Q(9,9) = 100.0f;
+    Q(10,10) = 100.0f;
+    Q(11,11) = 100.0f;
+    Q(12,12) = 100.0f;
+    
+    Ra(1,1) = 0.000020f;
+    Ra(2,2) = 0.000020f;
+    Ra(3,3) = 0.000020f;
+    Ra(4,4) = 10.0f;
+    Ra(5,5) = 10.0f;
+    
+    Rm(1,1) = 5.0f;
+    Rm(2,2) = 5.0f;
+    Rm(3,3) = 5.0f;
+    
+    Qab(1,1) = 100.7f;
+    Qab(2,2) = 100.7f;
+    Qab(3,3) = 100.7f;
+    Qab(4,4) = 0.0f;
+    Qab(5,5) = 0.0f;  
+    
+    for(int i = 0; i<10;i++){
+        histffunc[i] = 0.0f;
+    }
+    histffuncindex = 0 ;
+    sigma2a = 0.000020f;
+    
+    
+}
+
+void ScErrStateEKF::updateQhat(Vector3 gyro, float att_dt)
+{
+    gyro -= gyroBias;
+    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 + (1.0f/6.0f*att_dt*att_dt*att_dt) * A * A * A;
+    qhat = phi * qhat;
+    float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
+    qhat *= (1.0f/ qnorm);
+}
+
+void ScErrStateEKF::updateErrState(Vector3 gyro, float att_dt)
+{
+    gyro -= gyroBias;
+    Matrix A(12,12);
+    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;
+    A(10,7) = 1.0f;
+    A(11,8) = 1.0f;
+    A(12,9) = 1.0f;
+    
+    Matrix phi = MatrixMath::Eye(12) + A * att_dt + (0.5f*att_dt*att_dt) * A * A + (1.0f/6.0f*att_dt*att_dt*att_dt) * A * 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::updateAccMeasures(Vector3 acc, Vector3 accref)
+{
+    //acc -= accBias;
+    //acc = acc/acc.Norm();
+    //accref = accref/accref.Norm();
+    
+    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(5,12);
+    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,10) = 1.0f;
+    H(5,12) = 1.0f;
+    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra+Qab);
+    Matrix accmat(3,1);
+    accmat << acc.x << acc.y << acc.z;
+    Matrix gref(3,1);
+    gref << 0.0f << 0.0f << accref.z;
+    Matrix zacc = accmat-dcm*gref;
+    Matrix z(5,1);
+    z << zacc(1,1) << zacc(2,1) << zacc(3,1) << 0.0f << 0.0f;
+    Matrix corrVal =  K * (z-H*errState);
+    errState = errState + corrVal;
+    Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Ra+Qab)*MatrixMath::Transpose(K);
+}
+void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref)
+{
+    //acc = acc/acc.Norm();
+    //accref = accref/accref.Norm();
+    
+    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,9);
+    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(9)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(9)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K);
+}
+
+
+
+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,9);
+    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(9,9);
+    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(9,9);
+    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(9)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(9)-K*H)+K*(Rm)*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::fuseErr2Qhat()
+{
+    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);
+}
+
+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::triad(Vector3 fb, Vector3 fn, Vector3 mb, Vector3 mn){
+    Matrix W1(3,1);
+    W1 << fb.x << fb.y << fb.z;
+    Matrix W2(3,1);
+    W2 << mb.x << mb.y << mb.z;
+    
+    Matrix V1(3,1);
+    V1 << fn.x << fn.y << fn.z;
+    Matrix V2(3,1);
+    V2 << mn.x << mn.y << mn.z;
+    
+
+    Matrix Ou2(3,1);
+    Ou2 << W1( 2, 1 )*W2( 3, 1 )-W1( 3, 1 )*W2( 2, 1 ) << W1( 3, 1 )*W2( 1, 1 )-W1( 1, 1 )*W2( 3, 1 ) << W1( 1, 1 )*W2( 2, 1 )-W1( 2, 1 )*W2( 1, 1 );
+    Ou2 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2));
+    Matrix Ou3(3,1);
+    Ou3 << W1( 2, 1 )*Ou2( 3, 1 )-W1( 3, 1 )*Ou2( 2, 1 ) << W1( 3, 1 )*Ou2( 1, 1 )-W1( 1, 1 )*Ou2( 3, 1 ) << W1( 1, 1 )*Ou2( 2, 1 )-W1( 2, 1 )*Ou2( 1, 1 );
+    Ou3 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3));
+    Matrix R2(3,1);
+    R2  << V1( 2, 1 )*V2( 3, 1 )-V1( 3, 1 )*V2( 2, 1 ) << V1( 3, 1 )*V2( 1, 1 )-V1( 1, 1 )*V2( 3, 1 ) << V1( 1, 1 )*V2( 2, 1 )-V1( 2, 1 )*V2( 1, 1 );
+    R2 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2));
+    Matrix R3(3,1);
+    R3  << V1( 2, 1 )*R2( 3, 1 )-V1( 3, 1 )*R2( 2, 1 ) << V1( 3, 1 )*R2( 1, 1 )-V1( 1, 1 )*R2( 3, 1 ) << V1( 1, 1 )*R2( 2, 1 )-V1( 2, 1 )*R2( 1, 1 );
+    R3 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3));
+
+    Matrix Mou(3,3);
+    Mou << W1( 1, 1 ) << Ou2( 1, 1 ) << Ou3( 1, 1 )
+        << W1( 2, 1 ) << Ou2( 2, 1 ) << Ou3( 2, 1 )
+        << W1( 3, 1 ) << Ou2( 3, 1 ) << Ou3( 3, 1 );
+    Matrix Mr(3,3);
+    Mr << V1( 1, 1 ) << R2( 1, 1 ) << R3( 1, 1 )
+       << V1( 2, 1 ) << R2( 2, 1 ) << R3( 2, 1 )
+       << V1( 3, 1 ) << R2( 3, 1 ) << R3( 3, 1 );
+       
+    Matrix Cbn = Mr*MatrixMath::Transpose(Mou);
+
+    float sqtrp1 = sqrt(1.0f+Cbn( 1, 1 )+Cbn( 2, 2 )+Cbn( 3, 3 ));
+
+    qhat(1,1) = 0.5f*sqtrp1;
+    qhat(2,1) = -(Cbn( 2, 3 )-Cbn( 3, 2 ))/2.0f/sqtrp1;
+    qhat(3,1) = -(Cbn( 3, 1 )-Cbn( 1, 3 ))/2.0f/sqtrp1;
+    qhat(4,1) = -(Cbn( 1, 2 )-Cbn( 2, 1 ))/2.0f/sqtrp1;
+   
+    float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
+    qhat *= (1.0f/ qnorm);
+}
+
+
+Vector3 ScErrStateEKF::calcDynAcc(Vector3 acc, Vector3 accref)
+{
+    Matrix dcm(3,3);
+    computeDcm(dcm, qhat);
+    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;
+        
+}
+
+