Diff: main.cpp

Revision:

32:6ba2e5402f00

Parent:

31:3f91f4bfca8a

Child:

33:3025b16bccd2

--- a/main.cpp	Tue May 19 10:59:32 2015 +0000
+++ b/main.cpp	Sun May 24 06:38:54 2015 +0000
@@ -33,6 +33,11 @@
 //2015/05/17
 //Get_GPS()の中身longitudeの範囲138〜140に変更
 //
+//2015/05/24
+//Kalmanフィルターを十進数で計算するようにした。
+//Kalmanフィルターの計算式を変更した。
+//set_kalmanを追加した。
+//
 /**********************************************/
 
 #include "mbed.h"
@@ -88,12 +93,14 @@
 //For Kalman data
 //
 /////////////////////////////////////////
-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]={0};
-void Kalman(double Latitude,double Longitude,Adafruit_GPS *myGPS);
-
+#define FIRST_S2 0.000001
+#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 s2_R=FIRST_S2;//GPSセンサの分散
+double Kx=0,Ky=0;//カルマンゲイン
+double zx,zy;//観測値
+void Kalman(double Latitude,double Longitude);
 
 /////////////////////////////////////////
 //
@@ -142,7 +149,7 @@
     }
     //send normal data
 
-    
+ /*   
     //debug***************************************************
     printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
             agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
@@ -152,7 +159,7 @@
     for(int i = 0; i < RECEIVE_STATUS_COMMNAD_LENGTH; ++i) printf("%d ",agz.packetData[i]);
         printf("\n");
     //debug end***************************************************
-    
+ */   
     //Select Destination
     ZBTxRequest tx64request(send_Address,agz.packetData,agz.getPacketLength());
     //Send -> Base
@@ -167,27 +174,31 @@
 /////////////////////////////////////////
 
 void Get_GPS(Adafruit_GPS *myGPS){
-    static bool flag = true;
+    static int flag = 0;
     
     if (myGPS->fix) {
+        
         agz.nowStatus = GPS_AVAIL;
+        agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
         
-        if(flag){//初期値設定
-            if(myGPS->latitudeH>=36 && myGPS->latitudeH<=38 && myGPS->longitudeH>=138 && myGPS->longitudeH<=140){
-                flag = false;
-                x[0][0]=(double)myGPS->latitudeL;                        
-                x[0][1]=(double)myGPS->longitudeL;   
-            } 
+        if(flag < COUNTER_MAX){
+            flag++; 
+        }
+        if(flag >= 5){
+            x_prev = agz.get_agzPoint_lati();
+            y_prev = agz.get_agzPoint_longi();
         }
+            
+        if(flag >= 6){
+            Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi());
+            agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL);
+        } 
         
-        if(myGPS->longitudeH<138 || myGPS->longitudeH>140 || myGPS->latitudeH<36 || myGPS->latitudeH>38){
-            return;
-        }
-        //Kalman Filter
-        Kalman(myGPS->latitudeL,myGPS->longitudeL,myGPS);
-                
-        agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
-        agz.reNewRobotPointKalman(myGPS->latitudeKH,myGPS->latitudeKL,myGPS->longitudeKH,myGPS->longitudeKL);
+            printf("latitude:%f,longitude:%f\nlatitudeK:%f,longitudeK:%f\nCovlat:%f,Covlongi:%f\n",
+            agz.get_agzPoint_lati(),agz.get_agzPoint_longi(),
+            agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
+            agz.get_agzCov_lati(),agz.get_agzCov_longi()
+            );
     }
     else agz.nowStatus = GPS_UNAVAIL;
     
@@ -315,77 +326,45 @@
     
 }
 
+
 /////////////////////////////////////////
 //
 //Kalman Processing
 //
 /////////////////////////////////////////
-    
-void get_K(){
-  double temp[2][2]={
-    {sigma[0][0][0]+sigmaGPS[0][0],sigma[0][0][1]+sigmaGPS[0][1]},
-    {sigma[0][1][0]+sigmaGPS[1][0],sigma[0][1][1]+sigmaGPS[1][1]}
-  };
-  double ad_bc = temp[0][0]*temp[1][1]-temp[1][0]*temp[0][1];
-  K[1][0][0] = sigma[0][0][0]*(1/ad_bc)*(temp[1][1]);
-  K[1][1][1] = sigma[0][1][1]*(1/ad_bc)*(temp[0][0]);
-}
-
+void calc_Kalman(){
+  //calc Kalman gain
+  Kx = s2x_prev/(s2x_prev+s2_R);
+  Ky = s2y_prev/(s2y_prev+s2_R);
+  //estimate
+  x_cur = x_prev + Kx*(zx-x_prev);
+  y_cur = y_prev + Ky*(zy-y_prev);
+  //calc sigma
+  s2x_cur = s2x_prev-Kx*s2x_prev;
+  s2y_cur = s2y_prev-Ky*s2y_prev;
 
-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]={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){
-    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();
-    
+void Kalman(double Latitude,double Longitude){
+
+    zx = Latitude;
+    zy = Longitude;
+
+    calc_Kalman();
     
-    //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];
-        }
-    }
+    //更新
+    x_prev = x_cur;
+    y_prev = y_cur;
+    s2x_prev = s2x_cur;
+    s2y_prev = s2y_cur;
     
-    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
-    myGPS->longitudeKL=(long)x[1][1];//longitude after filtering
+    //agzPontKalmanとagzCovに格納する
+    agz.set_agzPointKalman_lati(x_cur);
+    agz.set_agzPointKalman_longi(y_cur);
+    agz.set_agzCov(s2x_cur,s2y_cur);
     
-    agz.set_agzCov(sigma[0][0][0],sigma[0][1][1]);
 }
 
-
-
 /////////////////////////////////////////
 //
 //Main Processing
@@ -469,10 +448,6 @@
                         Get_Status(SenderIDc,buf1);
                         break;
                     }
-                    case RECEIVE_KALMAN:{
-                        Get_Status_Kalman(SenderIDc, buf1);
-                        break;
-                    }
                     default:{
                         break;
                     }