Diff: Kalman/Kalman.h

Revision:

0:fbfafa6bf5f9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman/Kalman.h	Fri Apr 20 21:32:24 2012 +0000
@@ -0,0 +1,59 @@
+#include "rtos.h"
+//#include "Matrix.h"
+#include "motors.h"
+#include "RFSRF05.h"
+
+#include <tvmet/Matrix.h>
+#include <tvmet/Vector.h>
+using namespace tvmet;
+
+
+class Kalman {
+public:
+    enum measurement_t {SONAR1 = 0, SONAR2, SONAR3, IR1, IR2, IR3};
+    static const measurement_t maxmeasure = IR3;
+    
+    Kalman(Motors &motorsin);
+    
+    void predict();
+    void runupdate(measurement_t type, float value, float variance);
+    
+    //State variables
+    Vector<float, 3> X;
+    Matrix<float, 3, 3> P;
+    Mutex statelock;
+    
+    float SonarMeasures[3];
+    float IRMeasures[3];
+    
+private:
+
+    //Matrix<float, 3, 3> Q; //perhaps calculate on the fly? dependant on speed etc?
+    
+    RFSRF05 sonararray;
+    Motors& motors;
+    
+    Thread predictthread;
+    void predictloop();
+    static void predictloopwrapper(void const *argument){ ((Kalman*)argument)->predictloop(); }
+    RtosTimer predictticker;
+    
+//    Thread sonarthread;
+//    void sonarloop();
+//    static void sonarloopwrapper(void const *argument){ ((Kalman*)argument)->sonarloop(); }
+//    RtosTimer sonarticker;
+    
+    struct measurmentdata{
+        measurement_t mtype;
+        float value;
+        float variance;
+    } ;
+    
+    Mail <measurmentdata, 16> measureMQ;
+    
+    Thread updatethread;
+    void updateloop();
+    static void updateloopwrapper(void const *argument){ ((Kalman*)argument)->updateloop(); }
+    
+    
+};
\ No newline at end of file