![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
change kalman
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of Aigamozu_Base_ver3 by
Diff: main.cpp
- Revision:
- 13:82f4f00f11f6
- Parent:
- 12:83be8a3c212d
- Child:
- 14:3aa6d735a2fa
--- 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]); }