Diff: main.cpp

Revision:

13:82f4f00f11f6

Parent:

12:83be8a3c212d

Child:

14:3aa6d735a2fa

diff -r 83be8a3c212d -r 82f4f00f11f6 main.cpp
--- a/main.cpp	Mon May 18 02:38:35 2015 +0000
+++ b/main.cpp	Sun May 24 08:10:08 2015 +0000
@@ -20,6 +20,10 @@
 //2015/05/17
 //Get_GPS()の中身longitudeの範囲138〜140に変更
 //
+//2015/05/24
+//Kalmanフィルターを十進数で計算するようにした。
+//Kalmanフィルターの計算式を変更した。
+//set_kalmanを追加した。
 /**********************************************/
 
 #include "mbed.h"
@@ -35,7 +39,7 @@
 #define SIGMA_MIN 0.0001
 
 //************ID Number*****************
-const char MyID = 'a';
+const char MyID = 'b';
 //************ID Number*****************
 
 /////////////////////////////////////////
@@ -73,11 +77,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);
 
 
 /////////////////////////////////////////
@@ -104,7 +111,7 @@
                                     agz.get_agzPointKalman_lati(),agz.get_agzPointKalman_longi(),
                                     agz.get_agzCov_lati(),agz.get_agzCov_longi());
     
-    //debug***************************************************
+/*    //debug***************************************************
     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(),
@@ -113,7 +120,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
@@ -127,27 +134,28 @@
 /////////////////////////////////////////
 
 void Get_GPS(Adafruit_GPS *myGPS){
-    static bool flag = true;
+    static int flag = 0;
     
     if (myGPS->fix) {
         agz.nowStatus = GPS_AVAIL;
-        
-        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;   
-            } 
-        }
+        agz.reNewRobotPoint(myGPS->latitudeH,myGPS->latitudeL,myGPS->longitudeH,myGPS->longitudeL);
         
-        if(myGPS->longitudeH<138 || myGPS->longitudeH>140 || myGPS->latitudeH<37 || myGPS->latitudeH>38){
-            return;
+        if(flag < COUNTER_MAX){
+            flag++; 
+        }
+        if(flag == 15){
+            x_prev = agz.get_agzPoint_lati();
+            y_prev = agz.get_agzPoint_longi();
         }
-        //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);
+            
+        if(flag >= 16){
+            Kalman(agz.get_agzPoint_lati(), agz.get_agzPoint_longi());
+        } 
+        
+                printf("%.14lf %.14lf %.14lf %.14lf %.14le %.14le \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;
     
@@ -159,69 +167,37 @@
 //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 Kalman(double Latitude,double Longitude){
 
-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];
-        }
-    }   
-}
+    zx = Latitude;
+    zy = Longitude;
 
-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();
-    
+    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]);
 }