Diff: Processes/Kalman/Kalman.cpp

Revision:

49:665bdca0f2cd

Parent:

48:254b124cef02

Child:

72:7996aa8286ae

--- a/Processes/Kalman/Kalman.cpp	Fri Apr 12 17:03:53 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Fri Apr 12 21:07:00 2013 +0000
@@ -30,7 +30,7 @@
 Mutex statelock;
 
 float RawReadings[maxmeasure+1];
-int sensorseenflags = 0;
+volatile int sensorseenflags = 0;
 
 bool Kalman_inited = 0;
 
@@ -109,6 +109,8 @@
             frombrefoffset += constrainAngle(IR_Offsets[i] - IR_Offsets[refbeacon]);
         }
     }
+    
+    printf("Used IR info from %d beacons\r\n", cnt);
 
     X(3,0) = constrainAngle(IR_Offsets[refbeacon] + frombrefoffset/cnt);
 
@@ -123,7 +125,7 @@
     P = 0.02*0.02,  0,          0,          0,
         0,          0.02*0.02,  0,          0,
         0,          0,          0.4*0.4,    -0.4*0.4,
-        0,          0,          -0.4*0.4,   0.4*0.4;
+        0,          0,          -0.4*0.4,   0.4*0.4 + 0.05*0.05;
         
     statelock.unlock();
 
@@ -143,8 +145,9 @@
 void predictloop(void const*)
 {
 
-    OLED4 = !Printing::registerID(0, 3);
-    OLED4 = !Printing::registerID(1, 4);
+    OLED4 = !Printing::registerID(0, 3) || OLED4;
+    OLED4 = !Printing::registerID(1, 4) || OLED4;
+    OLED4 = !Printing::registerID(8, 1) || OLED4;
 
     float lastleft = 0;
     float lastright = 0;
@@ -222,6 +225,8 @@
         //Update Printing
         float statecpy[] = {X(0,0), X(1,0), X(2,0)};
         Printing::updateval(0, statecpy, 3);
+        
+        Printing::updateval(8, X(3,0));
 
         float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
         Printing::updateval(1, Pcpy, 4);
@@ -248,6 +253,7 @@
 void runupdate(measurement_t type, float value, float variance)
 {
     sensorseenflags |= 1<<type;
+    RawReadings[type] = value;
 
     if (Kalman_inited) {
         measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
@@ -364,7 +370,7 @@
                 }
 
                 Matrix<float, 4, 1> PHt (P * trans(H));
-                S = (H * PHt)(0,0) + variance;
+                S = (H * PHt)(0,0) + variance*10; //TODO: Temp Hack!
 
                 OLED3 = 0;
                 if (aborton2stddev && Y*Y > 4 * S) {