2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/Kalman/Kalman.cpp
- Revision:
- 49:665bdca0f2cd
- Parent:
- 48:254b124cef02
- Child:
- 72:7996aa8286ae
diff -r 254b124cef02 -r 665bdca0f2cd Processes/Kalman/Kalman.cpp --- 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) {