2015/05/17

Dependencies:   ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed

Fork of Aigamozu_Base_ver2_1 by aigamozu

Revision:
2:d97700414618
Parent:
1:ee2713435312
Child:
3:60bad3679b4b
diff -r ee2713435312 -r d97700414618 main.cpp
--- 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