File content as of revision 0:f3bf6c7e2283:

//***************************************************************************************
//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"

#include <tvmet/Matrix.h>
#include <tvmet/Vector.h>
using namespace tvmet;
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);


Kalman::Kalman(Motors &motorsin) :
        sonararray(p23,p14,p14,p14,p15,p15,p15,p5,p6,p7,p8,p11),
        motors(motorsin),
        predictthread(predictloopwrapper, this, osPriorityNormal, 512),
        predictticker( SIGTICKARGS(predictthread, 0x1) ),
//        sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
//        sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
        updatethread(updateloopwrapper, this, osPriorityNormal, 512) {

    //Initilising matrices

    // X = x, y, theta;
    X = 0.5, 0, 0;

    P = 1, 0, 0,
        0, 1, 0,
        0, 0, 0.04;

    Q = 0.002, 0, 0, //temporary matrix, Use dt!
        0, 0.002, 0,
        0, 0, 0.002;

    //measurment variance R is provided by each sensor when calling runupdate

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


//    predictticker.start(20);
//    sonarticker.start(50);


}


void Kalman::predictloop() {
    Matrix<float, 3, 3> F;
    int lastleft = motors.getEncoder1();
    int lastright = motors.getEncoder2();
    int leftenc;
    int rightenc;
    float dleft,dright;
    float dxp, dyp;
    float thetap,d,r;

    while (1) {
//        Thread::signal_wait(0x1);
        led1 = !led1;
        
        leftenc = motors.getEncoder1();
        rightenc = motors.getEncoder2();
        
        dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f;
        dright = motors.encoderToDistance(rightenc-lastright)/1000.0f;

        lastleft = leftenc;
        lastright = rightenc;


        //The below calculation are in body frame (where +x is forward)
        thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f);
        if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
            d = (dright + dleft)/2.0f;
            dxp = d*cos(thetap/2.0f);
            dyp = d*sin(thetap/2.0f);

        } else { //calculate circle arc
            //float r = (right + left) / (4.0f * PI * thetap);
            r = (dright + dleft) / (2.0f*thetap);
            dxp = abs(r)*sin(thetap);
            dyp = r - r*cos(thetap);
        }

        statelock.lock();

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

        //Linearising F around X
        F = 1, 0, (dxp * -sin(X(2)) - dyp * cos(X(2))),
            0, 1, (dxp * cos(X(2)) - dyp * sin(X(2))),
            0, 0, 1;


        P = F * P * trans(F) + Q;

        statelock.unlock();
        Thread::wait(20);

        //cout << "predict" << X << endl;
        //cout << P << endl;
    }
}

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


void Kalman::runupdate(measurement_t type, float value, float variance) {
    //printf("beacon %d dist %f\r\n", sonarid, dist);
    //led2 = !led2;

    measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
    if (measured) {
        measured->mtype = type;
        measured->value = value;
        measured->variance = variance;

        osStatus putret = measureMQ.put(measured);
        if (putret)
            led4 = 1;
        //    printf("putting in MQ error code %#x\r\n", putret);
    } else {
        led4 = 1;
        //printf("MQalloc returned NULL ptr\r\n");
    }

}

void Kalman::updateloop() {
    measurement_t type;
    float value,variance,rbx,rby,expecdist,Y;
    float dhdx,dhdy;
    bool aborton2stddev = false;

    Matrix<float, 1, 3> H;

    float S;
    Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );


    while (1) {
        led2 = !led2;

        osEvent evt = measureMQ.get();

        if (evt.status == osEventMail) {

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

            // don't forget to free the memory
            measureMQ.free(&measured);

            if (type <= maxmeasure) {

                if (type <= SONAR3) {

                    float dist = value / 1000.0f; //converting to m from mm
                    int sonarid = type;
                    aborton2stddev = true;

                    statelock.lock();
                    SonarMeasures[sonarid] = dist; //update the current sonar readings

                    rbx = X(0) - beaconpos[sonarid].x/1000.0f;
                    rby = X(1) - beaconpos[sonarid].y/1000.0f;

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

                    dhdx = rbx / expecdist;
                    dhdy = rby / expecdist;

                    H = dhdx, dhdy, 0;

                } else if (type <= IR3) {
                    
                    int IRidx = type-3;
                    
                    statelock.lock();
                    IRMeasures[IRidx] = value;
                    
                    rbx = X(0) - beaconpos[IRidx].x/1000.0f;
                    rby = X(1) - beaconpos[IRidx].y/1000.0f;
                    
                    float expecang = atan2(-rbx, -rby) - X(2);
                    Y = rectifyAng(value - expecang);
                    
                    float dstsq = rbx*rbx + rby*rby;
                    H = -rby/dstsq, rbx/dstsq, -1;
                    
                }

                Matrix<float, 3, 1> PH (P * trans(H));
                S = (H * PH)(0,0) + variance;

                if (aborton2stddev && Y*Y > 4 * S) {
                    statelock.unlock();
                    continue;
                }

                Matrix<float, 3, 1> K (PH * (1/S));

                //Updating state
                X += col(K, 0) * Y;
                X(2) = rectifyAng(X(2));

                P = (I3 - K * H) * P;

                statelock.unlock();

            }

        } else {
            led4 = 1;
            //printf("ERROR: in updateloop, code %#x", evt);
        }

    }

}