aigamozu / Mbed 2 deprecated Aigamozu_Robot_展示会

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ADXL345 AigamozuControlPackets_展示会 HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed

Fork of Aigamozu_Robot_ver4 by CanSat2015aizu

Diff: main.cpp

Revision:
4:f36986ceb73d
Parent:
3:1ac506a96fd6
Child:
5:522c47c78401
--- 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