Diff: run.cpp

Revision:

111:0fae4fbe2a80

Parent:

106:2d854e92cebb

Child:

112:6a33ea9f6fed

--- a/run.cpp	Fri Nov 12 12:56:48 2021 +0000
+++ b/run.cpp	Mon Nov 15 13:42:01 2021 +0000
@@ -31,11 +31,6 @@
     magref.x /= float(n_init);
     magref.y /= float(n_init);
     magref.z /= float(n_init);
-    if(hilFlag == false){
-        eskf.setMagField(magref.x,magref.y,-magref.z);
-    }else{
-        eskf.setMagField(magref.x,magref.y,magref.z);
-    }
     palt0 /= float(n_init);
     twelite.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]);
     //センサ正常性チェック
@@ -54,17 +49,19 @@
     eskf.setPhatAccBias(0.001f);
     eskf.setPhatGyroBias(0.001f);
     eskf.setPhatGravity(0.0000001f);
-    eskf.setPhatMagField(0.00001f);
+    eskf.setPhatMagBias(0.001f);
+    eskf.setPhatMagRadius(0.5f);
     
     eskf.setQVelocity(0.0025f);
     eskf.setQAngleError(0.00025f);
     eskf.setQAccBias(0.0000001f);
     eskf.setQGyroBias(0.000001f);
-    eskf.setQMagField(0.001f);
+    eskf.setQMagBias(0.000001f);
+    eskf.setQMagRadius(0.000001f);
     
-    Matrix Rgps(6,6);
+    Matrix Rgps(5,5);
     setDiag(Rgps,2.0f);
-    Rgps(6,6) = 100.0f;
+    //Rgps(6,6) = 100.0f;
     
     Matrix Rgpspos(3,3);
     setDiag(Rgpspos,2.0f);
@@ -74,8 +71,8 @@
     //Rgpsvel(3,3) = 10.0f;
     
     Matrix RImuConst(5,5);
-    setDiag(RImuConst,100.0f);
-    RImuConst(4,4) = 10.0f;
+    setDiag(RImuConst,50.0f);
+    RImuConst(4,4) = 1.0f;
     RImuConst(5,5) = 1.0f;
     
     Matrix Rmag(3,3);
@@ -109,12 +106,12 @@
             getGPSval();
         }
         if(gpsUpdateFlag == true){
-            eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),Rgps);
+            eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
             //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos);
             //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),Rgpsvel);
             
         }else{
-            //eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst);
+            eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst);
             //eskf.updateMag(MatrixMath::Vector2mat(mag),palt,Rmag);
         }
         PIDtick.loop();