Diff: solaESKF.cpp

Revision:

55:21611d4cf7e8

Parent:

54:cd514d9d4b19

Child:

56:c10f1168bd4a

--- a/solaESKF.cpp	Sun Nov 07 05:38:04 2021 +0000
+++ b/solaESKF.cpp	Mon Nov 08 09:19:55 2021 +0000
@@ -30,7 +30,7 @@
     setBlockDiag(Phat,0.1f,13,15);//gyro bias
     setBlockDiag(Phat,0.00000001f,16,18);//gravity 
     setBlockDiag(Q,0.00025f,4,6);//velocity
-    setBlockDiag(Q,0.005f/57.0,7,9);//angle error
+    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項なし
     
@@ -141,7 +141,7 @@
     Matrix gyrom = gyro - gyroBias;
     Matrix dcm(3,3);
     computeDcm(dcm, qhat);
-    Matrix a2v = -dcm*MatrixMath::Matrixcross(gyrom(1,1),gyrom(2,1),gyrom(3,1));
+    Matrix a2v = dcm*MatrixMath::Matrixcross(gyrom(1,1),gyrom(2,1),gyrom(3,1));
     
     Matrix H(2,nState);
     for (int i = 1; i < 3; i++){
@@ -163,7 +163,32 @@
     //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
     Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
 }
-
+void solaESKF::updateMag(Matrix mag,Matrix R)
+{
+    float magnorm = sqrt(mag(1,1)*mag(1,1)+mag(2,1)*mag(2,1)+mag(3,1)*mag(3,1));
+    Matrix magm = mag;
+    Matrix dcm(3,3);
+    computeDcm(dcm, qhat);
+    Matrix a2v = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
+    
+    Matrix H(2,nState);
+    for (int j = 1; j < 4; j++){
+        H(1,j+6) =  a2v(1,j);
+        H(2,j+6) =  a2v(2,j);
+    }
+    
+    //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
+    //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(2))*MatrixMath::Transpose(A));
+    Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+    
+    Matrix zmag = dcm*magm;
+    Matrix z(2,1);
+    z << 0.5f-zmag(1,1) << -zmag(2,1) ;
+    errState =  K * z;
+    //Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
+    //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
+    Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
+}
 
 void solaESKF::updateGPSVelocity(Matrix velgps,Matrix R)
 {