Revision 37:26374d6066cb, committed 2015-05-30

Comitter:

kityann

Date:

Sat May 30 14:46:59 2015 +0000

Parent:

36:a11060f5199e

Commit message:

2015/05/30

Changed in this revision

--- a/main.cpp	Fri May 29 13:07:46 2015 +0000
+++ b/main.cpp	Sat May 30 14:46:59 2015 +0000
@@ -43,6 +43,9 @@
 //
 //2015/05/29
 //auto_Move関数の実装とAigamozuControlPackets内にcontrol_Mortor関数の実装
+//
+//2015/05/30
+//新カルマンフィルタの実装
 /**********************************************/
 
 #include "mbed.h"
@@ -99,13 +102,15 @@
 //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);
@@ -393,14 +398,14 @@
 /////////////////////////////////////////
 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);
 
 }