aigamozu / Mbed 2 deprecated Aigamozu_Robot_ver3

2015/05/14

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

Fork of Aigamozu_Robot_ver2_2 by aigamozu

Files at this revision

API Documentation at this revision

Revision 4:f36986ceb73d, committed 2015-05-13

Comitter:
kityann
Date:
Wed May 13 08:43:55 2015 +0000
Parent:
3:1ac506a96fd6
Child:
5:522c47c78401
Commit message:
2015/05/13

Changed in this revision

AigamozuControlPackets.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/AigamozuControlPackets.lib	Wed May 13 06:29:21 2015 +0000
+++ b/AigamozuControlPackets.lib	Wed May 13 08:43:55 2015 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/teams/aigamozu/code/AigamozuControlPackets/#fec2d6dec897
+http://mbed.org/teams/aigamozu/code/AigamozuControlPackets/#e5dcf0d15879

--- a/main.cpp	Wed May 13 06:29:21 2015 +0000
+++ b/main.cpp	Wed May 13 08:43:55 2015 +0000
@@ -13,7 +13,9 @@
 //2015/05/11
 //ロボットプログラムの作成
 //
-//
+//2015/05/13
+//カルマンフィルタの共分散の値を0.0001以下にならないようにした
+//共分散の値を10進数に変換するようにした
 //
 /**********************************************/
 
@@ -25,6 +27,8 @@
 #include "aigamozuSetting.h"
 #include "Kalman.h"
 
+#define SIGMA_MIN 0.0001
+
 //************ID Number*****************
 //Robot   ID: 'A' ~ 'Z'
 //Base    ID: 'a' ~ 'a'
@@ -346,6 +350,9 @@
         }
     }
             
+    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
     myGPS->latitudeKL=(long)x[1][0];//latitude after filtering