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

--- 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;