Diff: main.cpp

Revision:

2:d97700414618

Parent:

1:ee2713435312

Child:

3:60bad3679b4b

--- a/main.cpp	Tue May 12 11:36:34 2015 +0000
+++ b/main.cpp	Wed May 13 08:41:58 2015 +0000
@@ -13,8 +13,9 @@
 //2015/05/11
 //ベースプログラムの作成
 //
-//
-//
+//2015/05/13
+//カルマンフィルタの共分散の値を0.0001以下にならないようにした
+//共分散の値を10進数に変換するようにした
 /**********************************************/
 
 #include "mbed.h"
@@ -26,6 +27,9 @@
 #include "agz_common.h"
 #include "Kalman.h"
 
+
+#define SIGMA_MIN 0.0001
+
 //************ID Number*****************
 const char MyID = 'D';
 //************ID Number*****************
@@ -215,6 +219,9 @@
             sigma[0][i][j]=sigma[1][i][j];
         }
     }
+    
+    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