Diff: solaESKF.cpp

Revision:

80:b241c058df83

Parent:

79:365ea9277167

Child:

81:230a3d2b0493

--- a/solaESKF.cpp	Fri Dec 10 10:42:09 2021 +0000
+++ b/solaESKF.cpp	Fri Jun 24 05:43:46 2022 +0000
@@ -58,7 +58,7 @@
     Matrix3f a2v = -dcm*solaESKF::Matrixcross(accm)*att_dt;
     Matrix3f a2v2 = 0.5f*a2v*att_dt;
 
-    MatrixXf Fx(nState,nState);
+    MatrixXf Fx = MatrixXf::Zero(nState, nState);
     //position
     Fx(0,0) =  1.0f;
     Fx(1,1) =  1.0f;
@@ -119,7 +119,7 @@
     Fx(16,16) =  1.0f;
     Fx(17,17) =  1.0f;
 
-    errState = Fx * errState;
+    //errState = Fx * errState;
     Phat = Fx*Phat*Fx.transpose();
     for (int i = 0; i < nState; i++){
         if(i>2 && i<9){
@@ -139,7 +139,7 @@
     Vector3f tdcm_g = tdcm*gravity;
     Matrix3f rotgrav = solaESKF::Matrixcross(tdcm_g);
     
-    MatrixXf H(3,nState);
+    MatrixXf H = MatrixXf::Zero(3,nState);
     for (int i = 0; i < 3; i++){
         for (int j = 0; j < 3; j++){
             H(i,j+6) =  rotgrav(i,j);
@@ -170,48 +170,48 @@
     float q3 = qhat(3);
 
     bool canUseA = false;
-    const float SA0 = 2*q3;
-    const float SA1 = 2*q2;
+    const float SA0 = 2.0f*q3;
+    const float SA1 = 2.0f*q2;
     const float SA2 = SA0*q0 + SA1*q1;
     const float SA3 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
     float SA4, SA5_inv;
     if ((SA3*SA3) > 1e-6f) {
-        SA4 = 1.0F/(SA3*SA3);
-        SA5_inv = SA2*SA2*SA4 + 1;
+        SA4 = 1.0f/(SA3*SA3);
+        SA5_inv = SA2*SA2*SA4 + 1.0f;
         canUseA = std::abs(SA5_inv) > 1e-6f;
     }
 
     bool canUseB = false;
-    const float SB0 = 2*q0;
-    const float SB1 = 2*q1;
+    const float SB0 = 2.0f*q0;
+    const float SB1 = 2.0f*q1;
     const float SB2 = SB0*q3 + SB1*q2;
     const float SB4 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
     float SB3, SB5_inv;
     if ((SB2*SB2) > 1e-6f) {
-        SB3 = 1.0F/(SB2*SB2);
+        SB3 = 1.0f/(SB2*SB2);
         SB5_inv = SB3*SB4*SB4 + 1;
         canUseB = std::abs(SB5_inv) > 1e-6f;
     }
 
-    Matrix<float, 1, 4> Hh;
+    MatrixXf Hh = MatrixXf::Zero(1,4);
 
     if (canUseA && (!canUseB || std::abs(SA5_inv) >= std::abs(SB5_inv))) {
-        const float SA5 = 1.0F/SA5_inv;
-        const float SA6 = 1.0F/SA3;
+        const float SA5 = 1.0f/SA5_inv;
+        const float SA6 = 1.0f/SA3;
         const float SA7 = SA2*SA4;
-        const float SA8 = 2*SA7;
-        const float SA9 = 2*SA6;
+        const float SA8 = 2.0f*SA7;
+        const float SA9 = 2.0f*SA6;
 
         Hh(0,0) = SA5*(SA0*SA6 - SA8*q0);
         Hh(0,1) = SA5*(SA1*SA6 - SA8*q1);
         Hh(0,2) = SA5*(SA1*SA7 + SA9*q1);
         Hh(0,3) = SA5*(SA0*SA7 + SA9*q0);
     } else if (canUseB && (!canUseA || std::abs(SB5_inv) > std::abs(SA5_inv))) {
-        const float SB5 = 1.0F/SB5_inv;
-        const float SB6 = 1.0F/SB2;
+        const float SB5 = 1.0f/SB5_inv;
+        const float SB6 = 1.0f/SB2;
         const float SB7 = SB3*SB4;
-        const float SB8 = 2*SB7;
-        const float SB9 = 2*SB6;
+        const float SB8 = 2.0f*SB7;
+        const float SB9 = 2.0f*SB6;
 
         Hh(0,0) = -SB5*(SB0*SB6 - SB8*q3);
         Hh(0,1) = -SB5*(SB1*SB6 - SB8*q2);
@@ -221,14 +221,14 @@
         return;
     }
     
-    Matrix<float, 4, 3> Hdq;
+    MatrixXf Hdq = MatrixXf::Zero(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<float, 1, 3> Hpart = Hh*Hdq;
-    MatrixXf H(1,nState);
+    MatrixXf  Hpart = Hh*Hdq;
+    MatrixXf H=MatrixXf::Zero(1,nState);
     for(int j=0; j<3; j++){
         H(0,j+6) = Hpart(0,j);
     }
@@ -242,9 +242,110 @@
     
     fuseErr2Nominal();
 }
+
+void solaESKF::updateIMU(Vector3f acc,float heading, Matrix<float, 4, 4> R)
+{
+    
+    Vector3f accm = acc - accBias;
+    Matrix3f dcm;
+    computeDcm(dcm, qhat);
+    Matrix3f tdcm = dcm.transpose();
+    Vector3f tdcm_g = tdcm*gravity;
+    Matrix3f rotgrav = solaESKF::Matrixcross(tdcm_g);
+    
+    MatrixXf H = MatrixXf::Zero(4,nState);
+    for (int i = 0; i < 3; i++){
+        for (int j = 0; j < 3; j++){
+            H(i,j+6) =  rotgrav(i,j);
+            H(i,j+15) = tdcm(i,j);
+        }
+    }
+
+    H(0,9) =   -1.0f;
+    H(1,10) =  -1.0f;
+    H(2,11) =  -1.0f;
+
+    float q0 = qhat(0);
+    float q1 = qhat(1);
+    float q2 = qhat(2);
+    float q3 = qhat(3);
+
+    bool canUseA = false;
+    const float SA0 = 2.0f*q3;
+    const float SA1 = 2.0f*q2;
+    const float SA2 = SA0*q0 + SA1*q1;
+    const float SA3 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
+    float SA4, SA5_inv;
+    if ((SA3*SA3) > 1e-6f) {
+        SA4 = 1.0f/(SA3*SA3);
+        SA5_inv = SA2*SA2*SA4 + 1.0f;
+        canUseA = std::abs(SA5_inv) > 1e-6f;
+    }
+
+    bool canUseB = false;
+    const float SB0 = 2.0f*q0;
+    const float SB1 = 2.0f*q1;
+    const float SB2 = SB0*q3 + SB1*q2;
+    const float SB4 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
+    float SB3, SB5_inv;
+    if ((SB2*SB2) > 1e-6f) {
+        SB3 = 1.0f/(SB2*SB2);
+        SB5_inv = SB3*SB4*SB4 + 1;
+        canUseB = std::abs(SB5_inv) > 1e-6f;
+    }
+
+    MatrixXf Hh = MatrixXf::Zero(1,4);
+
+    if (canUseA && (!canUseB || std::abs(SA5_inv) >= std::abs(SB5_inv))) {
+        const float SA5 = 1.0f/SA5_inv;
+        const float SA6 = 1.0f/SA3;
+        const float SA7 = SA2*SA4;
+        const float SA8 = 2.0f*SA7;
+        const float SA9 = 2.0f*SA6;
+
+        Hh(0,0) = SA5*(SA0*SA6 - SA8*q0);
+        Hh(0,1) = SA5*(SA1*SA6 - SA8*q1);
+        Hh(0,2) = SA5*(SA1*SA7 + SA9*q1);
+        Hh(0,3) = SA5*(SA0*SA7 + SA9*q0);
+    } else if (canUseB && (!canUseA || std::abs(SB5_inv) > std::abs(SA5_inv))) {
+        const float SB5 = 1.0f/SB5_inv;
+        const float SB6 = 1.0f/SB2;
+        const float SB7 = SB3*SB4;
+        const float SB8 = 2.0f*SB7;
+        const float SB9 = 2.0f*SB6;
+
+        Hh(0,0) = -SB5*(SB0*SB6 - SB8*q3);
+        Hh(0,1) = -SB5*(SB1*SB6 - SB8*q2);
+        Hh(0,2) = -SB5*(-SB1*SB7 - SB9*q2);
+        Hh(0,3) = -SB5*(-SB0*SB7 - SB9*q3);
+    } else {
+        return;
+    }
+    
+    MatrixXf Hdq = MatrixXf::Zero(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;  
+    
+    MatrixXf  Hpart = Hh*Hdq;
+    for(int j=0; j<3; j++){
+        H(3,j+6) = Hpart(0,j);
+    }
+    
+    const float psi = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3));
+    Vector3f zacc = -accm-tdcm*gravity;
+    VectorXf z = VectorXf::Zero(4);
+    z <<zacc(0),zacc(1),zacc(2),std::atan2(std::sin(heading-psi), std::cos(heading-psi));
+    MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse();
+    errState =  K * z;
+    Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat;
+    
+    fuseErr2Nominal();
+}
 void solaESKF::updateGPSPosition(Vector3f posgps,float palt,Matrix3f R)
 {
-    MatrixXf H(3,nState);
+    MatrixXf H = MatrixXf::Zero(3,nState);
     H(0,0)  = 1.0f;
     H(1,1)  = 1.0f;
     H(2,2)  = 1.0f;
@@ -262,7 +363,7 @@
 }
 void solaESKF::updateGPSVelocity(Vector3f velgps,Matrix3f R)
 {
-    MatrixXf H(3,nState);
+    MatrixXf H = MatrixXf::Zero(3,nState);
     H(0,3)  = 1.0f;
     H(1,4)  = 1.0f;
     H(2,5)  = 1.0f;
@@ -279,9 +380,116 @@
     fuseErr2Nominal();
 }
 
+void solaESKF::updateWhole(Vector3f posgps, float palt, Vector3f velgps,Vector3f acc,float heading, MatrixXf R)
+{
+    MatrixXf H = MatrixXf::Zero(9,nState);
+    H(0,0)  = 1.0f;
+    H(1,1)  = 1.0f;
+    H(2,2)  = 1.0f;
+    H(3,3)  = 1.0f;
+    H(4,4)  = 1.0f;
+    
+    Vector3f accm = acc - accBias;
+    Matrix3f dcm;
+    computeDcm(dcm, qhat);
+    Matrix3f tdcm = dcm.transpose();
+    Vector3f tdcm_g = tdcm*gravity;
+    Matrix3f rotgrav = solaESKF::Matrixcross(tdcm_g);
+    
+    for (int i = 0; i < 3; i++){
+        for (int j = 0; j < 3; j++){
+            H(i+5,j+6) =  rotgrav(i,j);
+            H(i+5,j+15) = tdcm(i,j);
+        }
+    }
+
+    H(5,9) =   -1.0f;
+    H(6,10) =  -1.0f;
+    H(7,11) =  -1.0f;
+
+    float q0 = qhat(0);
+    float q1 = qhat(1);
+    float q2 = qhat(2);
+    float q3 = qhat(3);
+
+    bool canUseA = false;
+    const float SA0 = 2.0f*q3;
+    const float SA1 = 2.0f*q2;
+    const float SA2 = SA0*q0 + SA1*q1;
+    const float SA3 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
+    float SA4, SA5_inv;
+    if ((SA3*SA3) > 1e-6f) {
+        SA4 = 1.0f/(SA3*SA3);
+        SA5_inv = SA2*SA2*SA4 + 1.0f;
+        canUseA = std::abs(SA5_inv) > 1e-6f;
+    }
+
+    bool canUseB = false;
+    const float SB0 = 2.0f*q0;
+    const float SB1 = 2.0f*q1;
+    const float SB2 = SB0*q3 + SB1*q2;
+    const float SB4 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
+    float SB3, SB5_inv;
+    if ((SB2*SB2) > 1e-6f) {
+        SB3 = 1.0f/(SB2*SB2);
+        SB5_inv = SB3*SB4*SB4 + 1;
+        canUseB = std::abs(SB5_inv) > 1e-6f;
+    }
+
+    MatrixXf Hh = MatrixXf::Zero(1,4);
+
+    if (canUseA && (!canUseB || std::abs(SA5_inv) >= std::abs(SB5_inv))) {
+        const float SA5 = 1.0f/SA5_inv;
+        const float SA6 = 1.0f/SA3;
+        const float SA7 = SA2*SA4;
+        const float SA8 = 2.0f*SA7;
+        const float SA9 = 2.0f*SA6;
+
+        Hh(0,0) = SA5*(SA0*SA6 - SA8*q0);
+        Hh(0,1) = SA5*(SA1*SA6 - SA8*q1);
+        Hh(0,2) = SA5*(SA1*SA7 + SA9*q1);
+        Hh(0,3) = SA5*(SA0*SA7 + SA9*q0);
+    } else if (canUseB && (!canUseA || std::abs(SB5_inv) > std::abs(SA5_inv))) {
+        const float SB5 = 1.0f/SB5_inv;
+        const float SB6 = 1.0f/SB2;
+        const float SB7 = SB3*SB4;
+        const float SB8 = 2.0f*SB7;
+        const float SB9 = 2.0f*SB6;
+
+        Hh(0,0) = -SB5*(SB0*SB6 - SB8*q3);
+        Hh(0,1) = -SB5*(SB1*SB6 - SB8*q2);
+        Hh(0,2) = -SB5*(-SB1*SB7 - SB9*q2);
+        Hh(0,3) = -SB5*(-SB0*SB7 - SB9*q3);
+    } else {
+        return;
+    }
+    
+    MatrixXf Hdq = MatrixXf::Zero(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;  
+    
+    MatrixXf  Hpart = Hh*Hdq;
+    for(int j=0; j<3; j++){
+        H(8,j+6) = Hpart(0,j);
+    }
+    
+    const float psi = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3));
+    Vector3f zacc = -accm-tdcm*gravity;
+    VectorXf z = VectorXf::Zero(9);
+    z << posgps(0)-pihat(0), posgps(1)-pihat(1), palt-pihat(2), velgps(0)-vihat(0), velgps(1)-vihat(1), zacc(0),zacc(1),zacc(2),std::atan2(std::sin(heading-psi), std::cos(heading-psi));
+    MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse();
+    errState =  K * z;
+    Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat;
+    
+    fuseErr2Nominal();
+    
+}
+
 void solaESKF::updateGPS(Vector3f posgps, float palt, Vector3f velgps, MatrixXf R)
 {
-    MatrixXf H(5, nState);
+    MatrixXf H = MatrixXf::Zero(5,nState);
     H(0,0)  = 1.0f;
     H(1,1)  = 1.0f;
     H(2,2)  = 1.0f;
@@ -397,9 +605,9 @@
 Vector3f solaESKF::calcDynAcc(Vector3f acc)
 {
     Vector3f accm = acc - accBias;
-    Matrix3f tdcm(3,3);
-    computeDcm(tdcm, qhat);
-    tdcm = tdcm.transpose();
+    Matrix3f dcm;
+    computeDcm(dcm, qhat);
+    Matrix3f tdcm = dcm.transpose();
 
     Vector3f dynAcc = accm+tdcm*gravity;
     return dynAcc;
@@ -443,13 +651,42 @@
     return errState;
 }
 
-void solaESKF::setPhatPosition(float val)
+VectorXf solaESKF::getState()
 {
-    setBlockDiag(Phat, val, 0,2);
+    VectorXf state = VectorXf::Zero(nState+1);
+    for (int i = 0; i < 3; i++){
+        state(i) = pihat(i);
+        state(i+3) = vihat(i);
+    }
+    for (int i = 0; i < 4; i++){
+        state(i+6) = qhat(i);
+    }
+    for (int i = 0; i < 3; i++){
+        state(i+10) = accBias(i);
+        state(i+13) = gyroBias(i);
+        state(i+16) = gravity(i);
+    }
+    return state;
 }
-void solaESKF::setPhatVelocity(float val)
+
+VectorXf solaESKF::getVariance()
 {
-    setBlockDiag(Phat, val, 3,5);
+    VectorXf variance = VectorXf::Zero(nState);
+    for (int i = 0; i < nState; i++){
+        variance(i) = Phat(i,i);
+    }
+    return variance;
+}
+
+void solaESKF::setPhatPosition(float valNE,float valD)
+{
+    setBlockDiag(Phat, valNE, 0,2);
+    Phat(2,2) = valD;
+}
+void solaESKF::setPhatVelocity(float valNE,float valD)
+{
+    setBlockDiag(Phat, valNE, 3,5);
+    Phat(5,5) = valD;
 }
 void solaESKF::setPhatAngleError(float val)
 {
@@ -469,9 +706,10 @@
 }
 
 
-void solaESKF::setQVelocity(float val)
+void solaESKF::setQVelocity(float valNE,float valD)
 {
-    setBlockDiag(Q, val, 3, 5);
+    setBlockDiag(Q, valNE, 3, 5);
+    Q(5,5) = valD;
 }
 void solaESKF::setQAngleError(float val)
 {
@@ -486,14 +724,31 @@
     setBlockDiag(Q, val, 12, 14);
 }
 
-
-void solaESKF::setDiag(MatrixXf& mat, float val){
+void solaESKF::setDiag(Matrix3f& mat, float val){
+    for (int i = 0; i < mat.cols(); i++){
+            for (int j = 0; j < mat.rows(); j++){
+                mat(i,j) = 0.0f;
+            }
+    }
     for (int i = 0; i < mat.cols(); i++){
             mat(i,i) = val;
     }
 }
 
+void solaESKF::setDiag(MatrixXf& mat, float val){
+    for (int i = 0; i < mat.cols(); i++){
+        for (int j = 0; j < mat.rows(); j++){
+            mat(i,j) = 0.0f;
+        }
+    }
+    for (int i = 0; i < mat.cols(); i++)
+    {
+        mat(i, i) = val;
+    }
+}
+
 void solaESKF::setBlockDiag(MatrixXf& mat, float val, int startIndex, int endIndex){
+
     for (int i = startIndex; i < endIndex+1; i++){
             mat(i,i) = val;
     }