File content as of revision 9:08552997b544:

#ifndef KALMAN_H
#define KALMAN_H

#include "globals.h"


#include "rtos.h"
//#include "Matrix.h"
#include "encoders.h"
#include "RFSRF05.h"
#include "IR.h"
#include "ui.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(Encoders &encodersin,
           UI &uiin,
           PinName Sonar_Trig,
           PinName Sonar_Echo0,
           PinName Sonar_Echo1,
           PinName Sonar_Echo2,
           PinName Sonar_Echo3,
           PinName Sonar_Echo4,
           PinName Sonar_Echo5,
           PinName Sonar_SDI,
           PinName Sonar_SDO,
           PinName Sonar_SCK,
           PinName Sonar_NCS,
           PinName Sonar_NIRQ);

    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];
    float IR_Offset;
    float Sonar_Offset;
    Mutex InitLock;

    bool Kalman_init;

    //The IR is public so it's possible to print the offset in the print function
    IR ir;

    //Initialises the kalman filter
    void KalmanInit();
    
    // reset kalman states
    void KalmanReset();

private:

    //Sensor interfaces
    RFSRF05 sonararray;
    Encoders& encoders;
    UI& ui;

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


};

#endif //KALMAN_H