Diff: run.cpp

Revision:

106:2d854e92cebb

Parent:

104:20b8caa29185

Child:

111:0fae4fbe2a80

--- a/run.cpp	Wed Nov 10 06:35:10 2021 +0000
+++ b/run.cpp	Fri Nov 12 09:03:41 2021 +0000
@@ -36,8 +36,6 @@
     }else{
         eskf.setMagField(magref.x,magref.y,magref.z);
     }
-    gps.Update();
-    gps.CalculateUnit();
     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]);
     //センサ正常性チェック
@@ -64,14 +62,27 @@
     eskf.setQGyroBias(0.000001f);
     eskf.setQMagField(0.001f);
     
-    Matrix Rgps(5,5);
+    Matrix Rgps(6,6);
     setDiag(Rgps,2.0f);
+    Rgps(6,6) = 100.0f;
+    
+    Matrix Rgpspos(3,3);
+    setDiag(Rgpspos,2.0f);
+    
+    Matrix Rgpsvel(3,3);
+    setDiag(Rgpsvel,2.0f);
+    //Rgpsvel(3,3) = 10.0f;
     
     Matrix RImuConst(5,5);
-    setDiag(RImuConst,50.0f);
-    RImuConst(4,4) = 1.0f;
+    setDiag(RImuConst,100.0f);
+    RImuConst(4,4) = 10.0f;
     RImuConst(5,5) = 1.0f;
-    _t.start();
+    
+    Matrix Rmag(3,3);
+    Rmag(1,1) = 10.0f;
+    Rmag(2,2) = 1.0f;
+    Rmag(3,3) = 2.0f;
+        _t.start();
     float tgps = _t.read();
     while(1)
     {
@@ -86,7 +97,6 @@
         eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
         eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt);
 
-        
         if(hilFlag == true){
             if(tstart-tgps>0.2f){
                 getHilGPSval();
@@ -99,10 +109,13 @@
             getGPSval();
         }
         if(gpsUpdateFlag == true){
-            eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps);
+            eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),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();