Diff: main.cpp

Revision:

46:e042e9969f10

Parent:

45:28f97c304a49

Child:

47:332d0030195f

--- a/main.cpp	Mon Jun 06 03:17:52 2016 +0000
+++ b/main.cpp	Fri Jun 10 09:06:32 2016 +0000
@@ -60,7 +60,7 @@
 //Base    ID: 'a' ~ 'a'
 //manager ID: '0'(数字のゼロ)
 //
-const char MyID = 'C';
+const char MyID = 'D';
 //************ID Number*****************
 
 /////////////////////////////////////////
@@ -98,10 +98,11 @@
 //For Kalman data
 //
 /////////////////////////////////////////
-#define FIRST_S2 0.000001
+#define FIRST_S1 0.00001
+#define FIRST_S2 0.0000001
 #define COUNTER_MAX 10000
 double x_cur,x_prev,y_cur,y_prev;//緯度と経度の時刻tと時刻t-1での推定値
-double s2x_cur=FIRST_S2,s2x_prev=FIRST_S2,s2y_cur=FIRST_S2,s2y_prev=FIRST_S2;//緯度経度のの時刻tと時刻t-1での共分散
+double s2x_cur=FIRST_S1,s2x_prev=FIRST_S1,s2y_cur=FIRST_S1,s2y_prev=FIRST_S1;//緯度経度のの時刻tと時刻t-1での共分散
 double s2_R=FIRST_S2;//GPSセンサの分散
 double Kx=0,Ky=0;//カルマンゲイン
 double zx,zy;//観測値
@@ -380,6 +381,12 @@
     //calc sigma
     s2x_cur = s2x_prev-Kx*s2x_prev;
     s2y_cur = s2y_prev-Ky*s2y_prev;
+    if( s2x_cur < 0.00000001){
+         s2x_cur = 0.000000001;
+    }
+    if( s2y_cur < 0.00000001){
+         s2y_cur = 0.000000001;
+    }
 
 }