2015/05/29
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed
Fork of Aigamozu_Robot_ver4_1 by
Diff: main.cpp
- Revision:
- 32:6ba2e5402f00
- Parent:
- 31:3f91f4bfca8a
- Child:
- 33:3025b16bccd2
diff -r 3f91f4bfca8a -r 6ba2e5402f00 main.cpp --- 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; }