Commits

Jody McAdams committed 4c861df

updated to use Kalman filtering

Comments (0)

Files changed (1)

Sensors/Gyro/Gyro.ino

 // Twitter: @MegaJiXiang
 // Website: http://www.jodymcadams.com
 // Original code/instructions from: http://bildr.org/2011/06/l3g4200d-arduino/
+// Kalman filter code from: http://interactive-matter.eu/blog/2009/12/18/filtering-sensor-data-with-a-kalman-filter/
 // License: http://www.opensource.org/licenses/mit-license.php (Go crazy)
 // L3G4200D data sheet: http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/DATASHEET/CD00265057.pdf
 
 #include <Wire.h>
 
-#define kFilteringFactor 0.1f
+#define lowPassFilteringFactor 0.1f
+
+//Allow the gyroscope values to be filtered to reduce noise
 #define ENABLE_FILTERING true
 
+//If ENABLE_FILTERING is on, use Kalman filtering instead of low pass filtering
+#define USE_KALMAN_FILTER true
+
+//For Kalman filtering
+const float Kalman_SensorNoise = 8.0f;  //Adjust this based on your sensor
+
+typedef struct
+{
+  float q; //process noise covariance
+  float x; //value
+  float p; //estimation error covariance
+  float k; //kalman gain
+} kalman_state;
+
+void kalman_init(kalman_state* pOut_state, float q, float p, float initialValue);
+void kalman_update(kalman_state* state, float measurement);
+
+#if USE_KALMAN_FILTER
+kalman_state kalmanState_X;
+kalman_state kalmanState_Y;
+kalman_state kalmanState_Z;
+#endif
+
 #define CTRL_REG1 0x20
 #define CTRL_REG2 0x21
 #define CTRL_REG3 0x22
 float y = 0.0f;
 float z = 0.0f;
 
-void setup(){
+float lastTimeMS;
 
+//Program init
+void setup()
+{
   Wire.begin();
   Serial.begin(9600);
 
   Serial.println("starting up L3G4200D");
   setupL3G4200D(500); // Configure L3G4200  - 250, 500 or 2000 deg/sec
 
-  delay(1500); //wait for the sensor to be ready 
+  delay(1500); //wait for the sensor to be ready
+  
+  //Init Kalman states if needed
+  #if USE_KALMAN_FILTER
+    const float initialQ = 0.0625f;
+    kalman_init(&kalmanState_X,initialQ,0.0f,0.0f);
+    kalman_init(&kalmanState_Y,initialQ,0.0f,0.0f);
+    kalman_init(&kalmanState_Z,initialQ,0.0f,0.0f);
+  #endif
+  
+  lastTimeMS = millis();
 }
 
-void loop(){
-   getGyroValues();  // This will update x, y, and z with new values
-   
-  Serial.print("X:");
-  Serial.println(x);
+float spamTimer = -1.0f;
+float spamInterval = 0.125f; //seconds
+
+//Program loop
+void loop()
+{
+  const float currTimeMS = millis();
+  const float timeElapsed = (currTimeMS-lastTimeMS)/1000.0f;
+  lastTimeMS = currTimeMS;
+  
+  getGyroValues();  // This will update x, y, and z with new values
+  
+  spamTimer -= timeElapsed;
+  if(spamTimer < 0.0f)
+  {
+    spamTimer = spamInterval;
+    
+    Serial.print("X:");
+    Serial.print(x);
+    Serial.print(", ");
  
-  /*Serial.print("Y:");
-  Serial.println(y);
+    Serial.print("Y:");
+    Serial.print(y);
+    Serial.print(", ");
 
-  Serial.print("Z:");
-  Serial.println(z);*/
-
-  delay(100); //Just here to slow down the serial to make it more readable
+    Serial.print("Z:");
+    Serial.println(z);
+  }
 }
 
 void getGyroValues(){
   float zVal = scaleFactor * ((zMSB << 8) | zLSB);
   
   #if ENABLE_FILTERING
-    x = FilterInput(x,xVal);
-    y = FilterInput(y,yVal);
-    z = FilterInput(z,zVal);
+    #if USE_KALMAN_FILTER
+      kalman_update(&kalmanState_X,xVal);
+      kalman_update(&kalmanState_Y,yVal);
+      kalman_update(&kalmanState_Z,zVal);
+      
+      x = kalmanState_X.x;
+      y = kalmanState_Y.x;
+      z = kalmanState_Z.x;
+    #else
+      //Use low pass filter
+      x = FilterInput(x,xVal);
+      y = FilterInput(y,yVal);
+      z = FilterInput(z,zVal);
+    #endif
   #else
     x = xVal;
     y = yVal;
     return v;
 }
 
+//Low pass filtering
+
 float FilterInput(float currInput, float newInput)
 {
-  return (newInput * kFilteringFactor) + (currInput * (1.0f - kFilteringFactor));
+  return (newInput * lowPassFilteringFactor) + (currInput * (1.0f - lowPassFilteringFactor));
 }
+
+//Kalman filtering
+
+void kalman_init(kalman_state* pOut_state, float q, float p, float initialValue)
+{
+  pOut_state->q = q;
+  pOut_state->p = p;
+  pOut_state->x = initialValue;
+}
+
+void kalman_update(kalman_state* state, float measurement)
+{
+  //prediction update
+  //omit x = x
+  state->p = state->p + state->q;
+
+  //measurement update
+  state->k = state->p / (state->p + Kalman_SensorNoise);
+  state->x = state->x + state->k * (measurement - state->x);
+  state->p = (1.0f - state->k) * state->p;
+}
+