Diff: main.cpp

Revision:

4:dc2babaa1f61

Parent:

3:7affc8af6db6

Child:

5:45e5e9e3dff3

--- a/main.cpp	Mon Apr 13 11:33:36 2015 +0000
+++ b/main.cpp	Tue Apr 14 15:11:29 2015 +0000
@@ -17,6 +17,7 @@
 #include "agzIDLIST.h"
 #include "aigamozuSetting.h"
 #include "agz_common.h"
+#include "Kalman.h"
 
 /////////////////////////////////////////
 //
@@ -44,6 +45,59 @@
 /////////////////////////////////////////
 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
 
+
+
+/////////////////////////////////////////
+//
+//Kalman Processing
+//
+/////////////////////////////////////////
+
+
+double sigmaGPS[2][2] = {{250,0},{0,250}};
+double K[2][2][2]= {{{1,0},{0,1}},{{1,0},{0,1}}};
+double sigma[2][2][2]= {{{250,0},{0,250}},{{250,0},{0,250}}};
+double y[2],x[2][2];
+    
+void get_K(){
+    double ad_bc = (sigma[0][0][0]+sigmaGPS[0][0])*(sigma[0][1][1]+sigmaGPS[1][1])-(sigma[0][1][0]+sigmaGPS[1][0])*(sigma[0][0][1]+sigmaGPS[0][1]);
+    K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(sigma[1][1][1]+sigmaGPS[1][1]);
+    K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(sigma[1][0][0]+sigmaGPS[0][0]);
+}
+
+void get_x(){
+    x[1][0] = x[0][0]+K[1][0][0]*(y[0]-x[0][0]);
+    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];
+        }
+    }   
+}
+
+void Kalman(double Latitude,double Longitude){
+    y[0] = Latitude;
+    y[1] = Longitude;
+    //K[t+1] = sigma[t]*Inverse[sigma[t]+sigmaGPS];
+    get_K();
+    //x[t+1] = x[t]+K[t+1]*(y[t*1]-x[t]);
+    get_x();
+    //sigma[t+1] = sigma[t]-K[t+1]*sigma[t];
+    get_sigma();
+}
+
+
 /////////////////////////////////////////
 //
 //Main Processing
@@ -72,7 +126,7 @@
                                         XBeeAddress64(BASE3_32H,BASE3_32L), XBeeAddress64(BASE4_32H,BASE4_32L)};
     XBeeAddress64 robot_Address = XBeeAddress64(ROBOT1_32H, ROBOT1_32L);
     int id;
-  
+    bool flag = true;
     
     //GPS Send Command
     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); 
@@ -138,7 +192,7 @@
                         robot[id].set_LatitudeH(&buf1[13]);
                         robot[id].set_LatitudeL(&buf1[17]);
                         robot[id].set_LongitudeH(&buf1[21]);
-                        robot[id].set_LongitudeL(&buf1[25]); 
+                        robot[id].set_LongitudeL(&buf1[25]);
                         
                         agz.reNewBasePoint(id,robot[id].get_LatitudeH(),robot[id].get_LatitudeL(),robot[id].get_LongitudeH(),robot[id].get_LongitudeL());
                     /*   
@@ -170,6 +224,27 @@
             refresh_Timer.reset();
             if (myGPS.fix) {
                 agz.nowStatus = GPS_AVAIL;
+                
+                if(flag){
+                    if(myGPS.longitudeH!=0 || myGPS.latitudeH!=0){
+                        flag = false;
+                        x[0][0]=(double)myGPS.longitudeL;                        
+                        x[0][1]=(double)myGPS.latitudeL;   
+                    } 
+                }
+                //Kalman Filter
+                Kalman(myGPS.longitudeL,myGPS.latitudeL);
+                //kousinn
+                for(int i = 0;i < 2;i++){
+                    for(int j = 0;j < 2;j++){
+                        K[0][i][j]=K[1][i][j];
+                        x[0][i]=x[1][i];
+                        sigma[0][i][j]=sigma[1][i][j];
+                    }
+                }
+                myGPS.longitudeL=(long)x[1][0];//longitude after filtering
+                myGPS.latitudeL=(long)x[1][1];//latitude after filtering
+                
                 agz.reNewRobotPoint(myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
                 //printf("state = %d\n",agz.nowMode);
                 //printf("my %ld, %ld, %ld, %ld\n", myGPS.longitudeH,myGPS.longitudeL,myGPS.latitudeH,myGPS.latitudeL);
@@ -178,6 +253,7 @@
             
         } 
         
+        
         //get base GPS
         if( collect_Timer.read_ms() >= collect_Time){
             collect_Timer.reset();