Diff: solaESKF.cpp

Revision:

62:5519d34eb6e8

Parent:

61:5e5c4fe12440

Child:

63:74a9565a0141

diff -r 5e5c4fe12440 -r 5519d34eb6e8 solaESKF.cpp
--- a/solaESKF.cpp	Tue Nov 16 13:56:44 2021 +0000
+++ b/solaESKF.cpp	Tue Nov 16 14:17:40 2021 +0000
@@ -3,17 +3,19 @@
 
 
 solaESKF::solaESKF()
-    :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),errState(16,1),Phat(16,16),Q(16,16)
-{ 
+    :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),magBias(3,1),errState(20,1),Phat(20,20),Q(20,20)
+{
     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();    
-    
+    nState = errState.getRows();
+
     for (int i = 1; i < nState+1; i++){
         errState(i,1) = 0.0f;
         for (int j = 1; j < nState+1; j++){
@@ -21,18 +23,21 @@
             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(Phat,0.00000001f,16,16);//gravity
+    setBlockDiag(Phat,0.001f,17,19);//magBias
+    setBlockDiag(Phat,0.001f,20,20);//magRadius
     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項なし
-    
+    setBlockDiag(Q,0.001f,13,15);//gyro bias
+    setBlockDiag(Q,0.001f,17,19);//magRadius
+    setBlockDiag(Q,0.0001f,20,20);//magRadius  //positionとgravityはQ項なし
     
 }
 
@@ -141,7 +146,7 @@
 }
 void solaESKF::updateMag(Matrix mag,Matrix R)
 {
-    Matrix magm = mag;
+    Matrix magm = mag-magBias;
     Matrix dcm(3,3);
     computeDcm(dcm, qhat);
     Matrix tdcm = MatrixMath::Transpose(dcm);
@@ -151,46 +156,24 @@
     
     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 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));
 
     for(int j = 1; j < 4; j++){
         H(1,j+6) =  rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx;
         H(2,j+6) =  rotmag(2,j);
-        H(3,j+6) =  rotmag(3,j)-(rotmag(3,j))/hz;
-    }
-    
-    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);
-        }
+        //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);
     }
-    
-    Matrix K = (Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+R);
-    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);
-        }
-    }
-    Kmod = kpart*Kmod;
-    for(int i = 1; i<nState+1; i++){
-        for(int j = 1;j<4;j++){
-            if(i>6 && i<10){
-                K(i,j) = Kmod(i-6,j);
-            }else{
-                K(i,j) = 0.0f;
-            }
-        }
-    }
+    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(3,1);
-    z << -(magned(1,1) - hx) << -magned(2,1) <<-(magned(3,1) - hz) ;
-    //twelite.printf("%f %f %f\r\n",z(1,1),z(2,1),z(3,1));
+    z << -(magned(1,1) - hx) << -magned(2,1) <<-(magRadius-a) ;
+    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
     errState =  K * z;
     Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
     
@@ -260,6 +243,12 @@
     gravity(2,1) = 0.0f;
     gravity(3,1) += errState(16,1);
     
+    //Magnetic bias
+    magBias(1,1) += errState(17,1);
+    magBias(2,1) += errState(18,1);
+    magBias(3,1) += errState(19,1);
+    magRadius += errState(20,1);
+    
     
     for (int i = 1; i < nState+1; i++){
         errState(i,1) = 0.0f;
@@ -320,6 +309,7 @@
     gravity(2,1) = gy;
     gravity(3,1) = gz;
 }
+
 Matrix solaESKF::getPihat()
 {
     return pihat;
@@ -344,6 +334,14 @@
 {
     return gravity;
 }
+Matrix solaESKF::getMagBias()
+{
+    return magBias;
+}
+float solaESKF::getMagRadius()
+{
+    return magRadius;
+}
 Matrix solaESKF::getErrState()
 {
     return errState;
@@ -373,6 +371,14 @@
 {
     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)
@@ -391,6 +397,14 @@
 {
     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){