change kalman

Dependencies:   ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed

Fork of Aigamozu_Robot_ver4 by aigamozu

Revision:
26:835042870f81
Parent:
25:ae5fab0946f2
Child:
27:2861016eaac6
--- a/main.cpp	Sun May 17 11:38:25 2015 +0000
+++ b/main.cpp	Sun May 17 14:29:44 2015 +0000
@@ -46,7 +46,7 @@
 //Base    ID: 'a' ~ 'a'
 //manager ID: '0'(数字のゼロ)
 //
-const char MyID = 'D';
+const char MyID = 'A';
 //************ID Number*****************
 
 /////////////////////////////////////////
@@ -333,21 +333,20 @@
   x[1][1] = x[0][1]+K[1][1][1]*(y[1]-x[0][1]);
 }
 
-
 void get_sigma(){
-    double temp[2][2];
-    for(int i=0;i<2;i++) {
-        for(int j=0;j<2;j++) {
-            for(int k=0;k<2;k++) {
-                temp[i][j]+=K[1][i][k]*sigma[0][k][j];
-            }
-        }
-   }
-    for(int i = 0;i < 2;i++){
-        for(int j = 0;j < 2;j++){
-            sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
-        }
-    }   
+   double temp[2][2]={0};
+   for(int i=0;i<2;i++) {
+       for(int j=0;j<2;j++) {
+           for(int k=0;k<2;k++) {
+               temp[i][j]+=K[1][i][k]*sigma[0][k][j];
+           }
+       }
+  }
+   for(int i = 0;i < 2;i++){
+       for(int j = 0;j < 2;j++){
+           sigma[1][i][j] = sigma[0][i][j]-temp[i][j];
+       }
+   }   
 }
 
 void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS){
@@ -410,7 +409,6 @@
     Timer collect_Timer;
     const int collect_Time = 2000; //when we collect 4 GPS point, we use 
     int collect_flag = 0;
-    char collect_state = 'a';
     
     char SenderIDc;
     //GPS Send Command
@@ -500,10 +498,8 @@
             Send_Request_Base(collect_flag);
             
             collect_flag++;
-            collect_state++;
 
             if(collect_flag == 4){
-                collect_state = 'a';
                 collect_flag = 0;
             }    
          }