Diff: main.cpp

Revision:

4:f36986ceb73d

Parent:

3:1ac506a96fd6

Child:

5:522c47c78401

--- a/main.cpp	Wed May 13 06:29:21 2015 +0000
+++ b/main.cpp	Wed May 13 08:43:55 2015 +0000
@@ -13,7 +13,9 @@
 //2015/05/11
 //ロボットプログラムの作成
 //
-//
+//2015/05/13
+//カルマンフィルタの共分散の値を0.0001以下にならないようにした
+//共分散の値を10進数に変換するようにした
 //
 /**********************************************/
 
@@ -25,6 +27,8 @@
 #include "aigamozuSetting.h"
 #include "Kalman.h"
 
+#define SIGMA_MIN 0.0001
+
 //************ID Number*****************
 //Robot   ID: 'A' ~ 'Z'
 //Base    ID: 'a' ~ 'a'
@@ -346,6 +350,9 @@
         }
     }
             
+    if(sigma[0][0][0] < SIGMA_MIN)sigma[0][0][0]=SIGMA_MIN;
+    if(sigma[0][1][1] < SIGMA_MIN)sigma[0][1][1]=SIGMA_MIN;
+    
     myGPS->latitudeKH=myGPS->latitudeH;//latitude after filtering
     myGPS->longitudeKH=myGPS->longitudeH;//longitude after filtering
     myGPS->latitudeKL=(long)x[1][0];//latitude after filtering