File content as of revision 0:fbfafa6bf5f9:

#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(); }
    
    
};