2015/05/14
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed
Fork of Aigamozu_Robot_ver2_2 by
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