Kalman.h

00001 #include "rtos.h"
00002 //#include "Matrix.h"
00003 #include "motors.h"
00004 #include "RFSRF05.h"
00005 
00006 #include <tvmet/Matrix.h>
00007 #include <tvmet/Vector.h>
00008 using namespace tvmet;
00009 
00010 
00011 class Kalman {
00012 public:
00013     enum measurement_t {SONAR1 = 0, SONAR2, SONAR3, IR1, IR2, IR3};
00014     static const measurement_t maxmeasure = IR3;
00015     
00016     Kalman(Motors &motorsin);
00017     
00018     void predict();
00019     void runupdate(measurement_t type, float value, float variance);
00020     
00021     //State variables
00022     Vector<float, 3> X;
00023     Matrix<float, 3, 3> P;
00024     Mutex statelock;
00025     
00026     float SonarMeasures[3];
00027     float IRMeasures[3];
00028     
00029 private:
00030 
00031     //Matrix<float, 3, 3> Q; //perhaps calculate on the fly? dependant on speed etc?
00032     
00033     RFSRF05 sonararray;
00034     Motors& motors;
00035     
00036     Thread predictthread;
00037     void predictloop();
00038     static void predictloopwrapper(void const *argument){ ((Kalman*)argument)->predictloop(); }
00039     RtosTimer predictticker;
00040     
00041 //    Thread sonarthread;
00042 //    void sonarloop();
00043 //    static void sonarloopwrapper(void const *argument){ ((Kalman*)argument)->sonarloop(); }
00044 //    RtosTimer sonarticker;
00045     
00046     struct measurmentdata{
00047         measurement_t mtype;
00048         float value;
00049         float variance;
00050     } ;
00051     
00052     Mail <measurmentdata, 16> measureMQ;
00053     
00054     Thread updatethread;
00055     void updateloop();
00056     static void updateloopwrapper(void const *argument){ ((Kalman*)argument)->updateloop(); }
00057     
00058     
00059 };