![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2015/06/09
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of Aigamozu_Base_ver3_1 by
Revision 16:ad381954fc13, committed 2015-06-08
- Comitter:
- kityann
- Date:
- Mon Jun 08 15:47:05 2015 +0000
- Parent:
- 15:35e3917fcbf5
- Commit message:
- 2015/06/09
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed May 27 12:12:04 2015 +0000 +++ b/main.cpp Mon Jun 08 15:47:05 2015 +0000 @@ -24,6 +24,9 @@ //Kalmanフィルターを十進数で計算するようにした。 //Kalmanフィルターの計算式を変更した。 //set_kalmanを追加した。 + +//2015/06/09 +//カルマンフィルタをロボットと同じ移動のブレを考えたものに変更した /**********************************************/ #include "mbed.h" @@ -77,12 +80,14 @@ //For Kalman data // ///////////////////////////////////////// -#define FIRST_S2 0.000001 +#define FIRST_S2_1 1.0e-8 +#define FIRST_S2_2 1.0e-6 #define COUNTER_MAX 10000 #define ERROR_RANGE 0.001 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 s2x_cur=FIRST_S2_1,s2x_prev=FIRST_S2_1,s2y_cur=FIRST_S2_1,s2y_prev=FIRST_S2_1;//緯度経度のの時刻tと時刻t-1での共分散 +double s2_R=FIRST_S2_2;//GPSセンサの分散 +double s2_Q=FIRST_S2_2; double Kx=0,Ky=0;//カルマンゲイン double zx,zy;//観測値 void Kalman(double Latitude,double Longitude); @@ -197,17 +202,18 @@ ///////////////////////////////////////// void calc_Kalman(){ //calc Kalman gain - Kx = s2x_prev/(s2x_prev+s2_R); - Ky = s2y_prev/(s2y_prev+s2_R); + Kx = (s2x_prev+s2_Q)/(s2x_prev+s2_R+s2_Q); + Ky = (s2y_prev+s2_Q)/(s2y_prev+s2_R+s2_Q); //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; + s2x_cur = (1-Kx)*(s2x_prev+s2_Q); + s2y_cur = (1-Ky)*(s2y_prev+s2_Q); } + void Kalman(double Latitude,double Longitude){ zx = Latitude;