File content as of revision 0:434fd09723be:

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

#include <tvmet/Matrix.h>
#include <tvmet/Vector.h>
using namespace tvmet;

Kalman::Kalman(Motors &motorsin,
               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) :
        ir(*this),
        sonararray(Sonar_Trig,
                   Sonar_Echo0,
                   Sonar_Echo1,
                   Sonar_Echo2,
                   Sonar_Echo3,
                   Sonar_Echo4,
                   Sonar_Echo5,
                   Sonar_SDI,
                   Sonar_SDO,
                   Sonar_SCK,
                   Sonar_NCS,
                   Sonar_NIRQ),
        motors(motorsin),
        ui(uiin),
        predictthread(predictloopwrapper, this, osPriorityNormal, 512),
        predictticker( SIGTICKARGS(predictthread, 0x1) ),
//        sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
//        sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
        updatethread(updateloopwrapper, this, osPriorityNormal, 512) {

    //Initilising offsets
    InitLock.lock();
    IR_Offset = 0;
    Sonar_Offset = 0;
    InitLock.unlock();


    //Initilising matrices

    // X = x, y, theta;
    if (Colour)
        X = 0.5, 0, 0;
    else
        X = 2.5, 0, PI;

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

    //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);

}


//Note: this init function assumes that the robot faces east, theta=0, in the +x direction
void Kalman::KalmanInit() {
    motors.stop();
    float SonarMeasuresx1000[3];
    float IRMeasuresloc[3];
    int beacon_cnt = 0;


// doesn't work since they break the ISR
    /*
    #ifdef ROBOT_PRIMARY
        LPC_UART3->FCR = LPC_UART3->FCR | 0x06;       // Flush the serial FIFO buffer / OR with FCR
    #else
        LPC_UART1->FCR = LPC_UART1->FCR | 0x06;       // Flush the serial FIFO buffer / OR with FCR
    #endif
    */
    // zeros the measurements
    for (int i = 0; i < 3; i++) {
        SonarMeasures[i] = 0;
        IRMeasures[i] = 0;
    }

    InitLock.lock();
    //zeros offsets
    IR_Offset = 0;
    Sonar_Offset = 0;
    InitLock.unlock();

    // attaches ir interrup
    ir.attachisr();

    //wating untill the IR has reved up and picked up some valid data
    //Thread::wait(1000);
    wait(2);

    //temporaraly disable IR updates
    ir.detachisr();

    //lock the state throughout the computation, as we will override the state at the end
    InitLock.lock();
    statelock.lock();



    SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
    SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
    SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
    IRMeasuresloc[0] = IRMeasures[0];
    IRMeasuresloc[1] = IRMeasures[1];
    IRMeasuresloc[2] = IRMeasures[2];
    //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);

    float d = beaconpos[2].y - beaconpos[1].y;
    float i = beaconpos[0].y - beaconpos[1].y;
    float j = beaconpos[0].x - beaconpos[1].x;
    float origin_x = beaconpos[1].x;
    float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
    float x_coor = origin_x + (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;

    //debug for trilateration
    printf("Cal at x: %0.4f, y: %0.4f \r\n",x_coor,y_coor );

    float Dist_Exp[3];
    for (int i = 0; i < 3; i++) {
        //Compute sonar offset
        Dist_Exp[i] = hypot(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
        Sonar_Offset += (SonarMeasuresx1000[i]-Dist_Exp[i])/3000.0f;

        //Compute IR offset
        float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
        if (!Colour)
        angle_est -= PI;
        //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
        // take average offset angle from valid readings
        if (IRMeasuresloc[i] != 0) {
            beacon_cnt ++;
            // changed to current angle - estimated angle
            float angle_temp = IRMeasuresloc[i] - angle_est;
            angle_temp -= (floor(angle_temp/(2*PI)))*2*PI;
            IR_Offset += angle_temp;
        }
    }
    IR_Offset /= float(beacon_cnt);

    //debug
    printf("Offsets IR: %0.4f, Sonar: %0.4f \r\n",IR_Offset*180/PI,Sonar_Offset*1000 );

    //statelock already locked
    X(0) = x_coor/1000.0f;
    X(1) = y_coor/1000.0f;
    
    if (Colour)
        X(2) = 0;
    else
        X(2) = PI;

    // unlocks mutexes
    InitLock.unlock();
    statelock.unlock();


    //reattach the IR processing
    ir.attachisr();
}


void Kalman::predictloop() {

    OLED4 = !ui.regid(0, 3);
    OLED4 = !ui.regid(1, 4);

    float lastleft = 0;
    float lastright = 0;

    while (1) {
        Thread::signal_wait(0x1);
        OLED1 = !OLED1;

        int leftenc = motors.getEncoder1();
        int rightenc = motors.getEncoder2();

        float dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f;
        float dright = motors.encoderToDistance(rightenc-lastright)/1000.0f;

        lastleft = leftenc;
        lastright = rightenc;


        //The below calculation are in body frame (where +x is forward)
        float dxp, dyp,d,r;
        float 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();

        float tempX2 = X(2);
        //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
        float avgX2 = (X(2) + tempX2)/2.0f;
        Matrix<float, 3, 3> F;
        F = 1, 0, (dxp * -sin(avgX2) - dyp * cos(avgX2)),
            0, 1, (dxp * cos(avgX2) - dyp * sin(avgX2)),
            0, 0, 1;

        //Generating forward and rotational variance
        float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f;
        float varang = varperang * abs(thetap);
        float varxydt = xyvarpertime * PREDICTPERIOD/1000.0f;
        float varangdt = angvarpertime * PREDICTPERIOD/1000.0f;

        //Rotating into cartesian frame
        Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
        Qsub = varfwd + varxydt, 0,
               0, varxydt;

        Qrot = Rotmatrix(X(2));

        Qsubrot = Qrot * Qsub * trans(Qrot);

        //Generate Q
        Matrix<float, 3, 3> Q;//(Qsubrot);
        Q = Qsubrot(0,0), Qsubrot(0,1), 0,
            Qsubrot(1,0), Qsubrot(1,1), 0,
            0, 0, varang + varangdt;

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

        //Update UI
        float statecpy[] = {X(0), X(1), X(2)};
        ui.updateval(0, statecpy, 3);

        float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
        ui.updateval(1, Pcpy, 4);

        statelock.unlock();
    }
}

//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)
            OLED4 = 1;
        //    printf("putting in MQ error code %#x\r\n", putret);
    } else {
        OLED4 = 1;
        //printf("MQalloc returned NULL ptr\r\n");
    }

}

void Kalman::updateloop() {

    //sonar Y chanels
    ui.regid(2, 1);
    ui.regid(3, 1);
    ui.regid(4, 1);

    //IR Y chanels
    ui.regid(5, 1);
    ui.regid(6, 1);
    ui.regid(7, 1);

    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) {
        OLED2 = !OLED2;

        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) {

                    InitLock.lock();
                    float dist = value / 1000.0f - Sonar_Offset; //converting to m from mm,subtract the offset
                    InitLock.unlock();

                    int sonarid = type;
                    aborton2stddev = true;

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

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

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

                    //send to ui
                    ui.updateval(sonarid+2, Y);

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

                    H = dhdx, dhdy, 0;

                } else if (type <= IR3) {

                    aborton2stddev = false;
                    int IRidx = type-3;

                    // subtract the IR offset
                    InitLock.lock();
                    value -= IR_Offset;
                    InitLock.unlock();

                    statelock.lock();
                    IRMeasures[IRidx] = value;

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

                    float expecang = atan2(-rby, -rbx) - X(2);
                    Y = rectifyAng(value - expecang);

                    //send to ui
                    ui.updateval(IRidx + 5, Y);

                    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 {
            OLED4 = 1;
            //printf("ERROR: in updateloop, code %#x", evt);
        }

    }

}

// reset kalman states
void Kalman::KalmanReset() {
    float SonarMeasuresx1000[3];
    statelock.lock();
    SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
    SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
    SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
    //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);

    float d = beaconpos[2].y - beaconpos[1].y;
    float i = beaconpos[0].y - beaconpos[1].y;
    float j = beaconpos[0].x - beaconpos[1].x;
    float origin_x = beaconpos[1].x;
    float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
    float x_coor = origin_x +(SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;

    //statelock already locked
    X(0) = x_coor/1000.0f;
    X(1) = y_coor/1000.0f;
   
    

/*    if (Colour){
        X(0) = 0.2;
        X(1) = 0.2;
        //X(2) = 0;
        }
    else {
        X(0) = 2.8;
        X(1) = 0.2;
        //X(2) = PI;
    }
    */
    P = 0.05, 0, 0,
        0, 0.05, 0,
        0, 0, 0.04;

    // unlocks mutexes
    statelock.unlock();

}