Diff: solaESKF.cpp

Revision:

69:2b2815603e5a

Parent:

68:264a7e0e4a29

--- a/solaESKF.cpp	Thu Nov 18 10:10:07 2021 +0000
+++ b/solaESKF.cpp	Fri Nov 19 05:44:14 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),magBias(3,1),errState(19,1),Phat(19,19),Q(19,19)
 {
     pihat << 0.0f << 0.0f << 0.0f;
     vihat << 0.0f << 0.0f << 0.0f;
@@ -12,7 +12,6 @@
     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();
 
@@ -31,13 +30,11 @@
     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(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,17,19);//mag bias //positionとgravityはQ項なし
     
 }
 
@@ -220,7 +217,7 @@
 void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
 {
     Matrix accm = acc - accBias;
-    Matrix magm = mag;
+    Matrix magm = mag-  magBias;
     Matrix dcm(3,3);
     computeDcm(dcm, qhat);
     Matrix tdcm = MatrixMath::Transpose(dcm);
@@ -228,7 +225,8 @@
     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(4,nState);
+    int nH = 4;
     for (int i = 1; i < 4; i++){
         for (int j = 1; j < 4; j++){
             H(i,j+6) =  rotgrav(i,j);
@@ -243,19 +241,19 @@
     Matrix magned = dcm*magm;
     float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
     
-    for(int j = 3; 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(4,3+6) =  rotmag(1,3)-(rotmag(1,3)+rotmag(2,3))/hx;
+    for(int j = 1; j < 4; j++){
+        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));
+    Matrix z(nH,1);
+    z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(zmag(1,1) - hx);
+    twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
     errState =  K * z;
     Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
     
@@ -263,28 +261,6 @@
 }
 
 
-void solaESKF::updateOtherConstraints(Matrix mag,float palt,Matrix R)
-{
-    Matrix magm = mag - magBias;
-    float a = sqrt(magm(1,1)*magm(1,1)+magm(2,1)*magm(2,1)+magm(3,1)*magm(3,1));
-    Matrix H(2,nState);
-    H(1,20) = -1.0f;
-    H(1,17) = -magm(1,1)/a;
-    H(1,18) = -magm(2,1)/a;
-    H(1,19) = -magm(3,1)/a;
-    H(2,3) = 1.0f;
-
-    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
-    
-    Matrix z(2,1);
-    z << -(a-magRadius) << palt-pihat(3,1);
-    //twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
-    errState =  K * z;
-    Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
-    
-    fuseErr2Nominal();
-}
-
 
 void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R)
 {
@@ -352,7 +328,6 @@
     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++){
@@ -443,10 +418,6 @@
 {
     return magBias;
 }
-float solaESKF::getMagRadius()
-{
-    return magRadius;
-}
 Matrix solaESKF::getErrState()
 {
     return errState;
@@ -480,10 +451,6 @@
 {
     setBlockDiag(Phat,val,17,19);
 }
-void solaESKF::setPhatMagRadius(float val)
-{
-    setBlockDiag(Phat,val,20,20);
-}
 
 
 void solaESKF::setQVelocity(float val)
@@ -506,10 +473,7 @@
 {
     setBlockDiag(Q,val,17,19);
 }
-void solaESKF::setQMagRadius(float val)
-{
-    setBlockDiag(Q,val,20,20);
-}
+
 
 
 void solaESKF::setDiag(Matrix& mat, float val){