File content as of revision 0:349dc9b0984f:

//***************************************************************************************
//Kalman Filter implementation
//***************************************************************************************
#include "Kalman.h"
//#include "rtos.h"
//#include "RFSRF05.h"
//#include "MatrixMath.h"
//#include "Matrix.h"
//#include "math.h"
//#include "globals.h"
//#include "motors.h"
//#include "system.h"


Kalman::Kalman() : //Motors &motorsin) :
        //X(3,1),
        //P(3,3),
        //Q(3,3),
        //sonararray(p13,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9){
        rflol(p5, p6, p7, p8, p9){
        //motors(motorsin) {
    //predictthread(predictloopwrapper, this),
    //predictticker( SIGTICKARGS(predictthread, 0x1) ), //(tfuncptr_t) (&Signalsetter::callback), osTimerPeriodic, (void*)(new Signalsetter(predictthread, 0x1)) ),
    //sonarthread(sonarloopwrapper, this),
    //sonarticker( SIGTICKARGS(sonarthread, 0x1) ){
    //updatethread(updateloopwrapper, this) {


    //Initilising matrices
    /*
    X << 500
    << 0
    << 0;

    P << 1000 << 0 << 0
    << 0 << 1000 << 0
    << 0 << 0 << 1000;

    Q << 1 << 0 << 0 //temporary matrix
    << 0 << 1 << 0
    << 0 << 0 << 1;

    */
    R = 100; //Preliminary

    //attach callback
    //sonararray.callbackobj = (DummyCT*)this;
    //sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance)) &Kalman::runupdate;

    //attach predict ticker
    //predictticker.attach(this, &Kalman::predictlauncher, 0.5); //20Hz for now

    //predictticker.start(5000);

    //predictthread = new Thread(predictloopwrapper, this);

}

/*
void Kalman::predictloop() {
    while (1) {
        Thread::signal_wait(0x1);
        predict();
    }
}
*/

/*
void Kalman::predict() {

    static int lastleft = 0;
    static int lastright = 0;

    int left = motors.encoderToDistance(motors.getEncoder1()) - lastleft;
    int right = motors.encoderToDistance(motors.getEncoder2()) - lastright;

    lastleft += left;
    lastright += right;

    float dxp, dyp;

    //The below calculation are in body frame (where +x is forward)
    float thetap = (right - left)*PI / (2.0f * robotCircumference);

    if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
        float d = (right + left)/2.0f;
        dxp = d*cos(thetap/2);
        dyp = d*sin(thetap/2);
    } else { //calculate circle arc
        //float r = (right + left) / (4.0f * PI * thetap);
        float r = (right+left) / (2.0f*thetap);
        dxp = abs(r)*sin(thetap);
        dyp = r - r*cos(thetap);
    }

    statelock.lock();

    //rotating to cartesian frame and updating state
    X(1,1) += dxp * cos(X(3,1)) - dyp * sin(X(3,1));
    X(2,1) += dxp * sin(X(3,1)) + dyp * cos(X(3,1));
    X(3,1) += thetap;

    //Linearising F around X
    Matrix F(3,3);
    F << 1 << 0 << (dxp * -sin(X(3,1)) - dyp * cos(X(3,1)))
    << 0 << 1 << (dxp * cos(X(3,1)) - dyp * sin(X(3,1)))
    << 0 << 0 << 1;

    //Updating P
    P = F * P * MatrixMath::Transpose(F) + Q;

    statelock.unlock();
}


void Kalman::sonarloop() {
    while (1) {
        Thread::signal_wait(0x1);
        sonararray.startRange();
    }
}


void Kalman::runupdate(int sonarid, float dist) {
    measurmentdata* measured = NULL;//(measurmentdata*)measureMQ.alloc();
    if (measured) {
        measured->mtype = (measurement_t)sonarid;
        measured->value = dist;
        int putret = NULL;//osStatus putret = measureMQ.put(measured);
        if (putret)
            printf("putting in MQ error code %#x\r\n", putret);
    } else {
        printf("MQalloc returned NULL ptr\r\n");
    }
}

void Kalman::updateloop() {

    while (1) {
        
        osEvent evt = measureMQ.get();
        if (evt.status == osEventMail) {

            measurmentdata& measured = *(measurmentdata*)evt.value.p;
            int sonarid = measured.mtype; //Note, may support more measurment types than sonar in the future!
            float dist = measured.value;

            statelock.lock();

            float rbx = X(1,1) - beaconpos[sonarid].x;
            float rby = X(2,1) - beaconpos[sonarid].y;

            float expecdist = sqrt(rbx*rbx + rby*rby);
            float Y = dist - expecdist;

            float dhdx = rbx / expecdist;
            float dhdy = rby / expecdist;
            Matrix H(1,3);
            H << dhdx << dhdy << 0;

            Matrix PH = P * MatrixMath::Transpose(H);
            float S = (H * PH)(1,1) + R;
            Matrix K = PH * (1/S);

            //Updating state
            X += K*Y;


            Matrix I3(3,3);
            I3 << 1 << 0 << 0
            << 0 << 1 << 0
            << 0 << 0 << 1;
            P = (I3 - K * H) * P;

            statelock.unlock();

            printf("u %.1f %.1f %.1f\r\n", X(1,1), X(2,1), X(3,1));
        } else {
            printf("ERROR: in updateloop, code %#x", evt);
        }
        
    }

}
*/