![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
change kalman
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST agz_common mbed
Fork of Aigamozu_Base_ver3 by
Diff: main.cpp
- 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