Diff: solaESKF.cpp

Revision:

61:5e5c4fe12440

Parent:

60:f4b4231a8d3f

Child:

62:5519d34eb6e8

Child:

64:f5b6256726f3

--- a/solaESKF.cpp	Tue Nov 16 01:20:50 2021 +0000
+++ b/solaESKF.cpp	Tue Nov 16 13:56:44 2021 +0000
@@ -3,7 +3,7 @@
 
 
 solaESKF::solaESKF()
-    :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(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 << 0.0f << 0.0f << 0.0f;
     vihat << 0.0f << 0.0f << 0.0f;
@@ -11,8 +11,6 @@
     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.5f;
 
     nState = errState.getRows();    
     
@@ -29,15 +27,11 @@
     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.001f,17,19);//magBias
-    setBlockDiag(Phat,0.001f,20,20);//magRadius 
+    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
-    setBlockDiag(Q,0.001f,17,19);//magRadius
-    setBlockDiag(Q,0.0001f,20,20);//magRadius  //positionとgravityはQ項なし
+    setBlockDiag(Q,0.001f,13,15);//gyro bias//positionとgravityはQ項なし
     
     
 }
@@ -115,18 +109,16 @@
     Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Q;
 }
 
-void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
+void solaESKF::updateAcc(Matrix acc,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);
+    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);
@@ -138,22 +130,67 @@
     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;
+    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(3,nState);
+    
     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));
+   
+
     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);
+        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);
+        }
     }
     
-    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));
+    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;
+            }
+        }
+    }
+    
+    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));
     errState =  K * z;
     Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
     
@@ -223,11 +260,6 @@
     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;
@@ -312,14 +344,6 @@
 {
     return gravity;
 }
-Matrix solaESKF::getMagBias()
-{
-    return magBias;
-}
-float solaESKF::getMagRadius()
-{
-    return magRadius;
-}
 Matrix solaESKF::getErrState()
 {
     return errState;
@@ -349,14 +373,6 @@
 {
     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)
@@ -375,14 +391,6 @@
 {
     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){
@@ -568,4 +576,49 @@
     //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();
+}
 */
\ No newline at end of file