File content as of revision 79:0d3140048526:

//***************************************************************************************
//Kalman Filter implementation
//***************************************************************************************
#include "Kalman.h"
#include "rtos.h"
#include "math.h"
#include "supportfuncs.h"
#include "Encoder.h"
#include "globals.h"
#include "Printing.h"

#include "tvmet/Matrix.h"
using namespace tvmet;



namespace Kalman
{

Ticker predictticker;

DigitalOut OLED4(LED4);
DigitalOut OLED3(LED3);
DigitalOut OLED1(LED1);
DigitalOut OLED2(LED2);

//State variables
Matrix<float, 4, 1> X;
Matrix<float, 4, 4> P;
Mutex statelock;

float RawReadings[maxmeasure+1];
volatile int sensorseenflags = 0;

bool Kalman_inited = 0;

struct measurmentdata {
    measurement_t mtype;
    float value;
    float variance;
};

Mail <measurmentdata, 16> measureMQ;

Thread* predict_thread_ptr = NULL;


//Note: this init function assumes that the robot faces east, theta=0, in the +x direction
void KalmanInit()
{
    printf("kalmaninit \r\n");

    //WARNING: HARDCODED! TODO: fix it so it works for both sides!

    printf("waiting for all sonar, and at least 1 IR\r\n");
    while( ((sensorseenflags & 0x7)^0x7) || !(sensorseenflags & 0x7<<3) );

#ifdef TEAM_RED
    //solve for our position (assume perfect bias)
    const float d = beaconpos[2].y - beaconpos[1].y;
    const float i = beaconpos[2].y - beaconpos[0].y;
    const float j = beaconpos[2].x - beaconpos[0].x;
    float r1 = RawReadings[SONAR2];
    float r2 = RawReadings[SONAR1];
    float r3 = RawReadings[SONAR0];
#endif
#ifdef  TEAM_BLUE
    const float d = beaconpos[1].y - beaconpos[2].y;
    const float i = beaconpos[0].y - beaconpos[2].y;
    const float j = beaconpos[0].x - beaconpos[2].x;
    float r1 = RawReadings[SONAR2];
    float r2 = RawReadings[SONAR1];
    float r3 = RawReadings[SONAR0];
#endif

    printf("ranges: 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", r1, r2, r3);

    float y_coor = (r1*r1-r2*r2+d*d)/(2*d);
    float x_coor = (r1*r1-r3*r3+i*i+j*j)/(2*j) - (i*y_coor)/j;

#ifdef TEAM_RED
    //coordinate system hack (for now)
    x_coor = beaconpos[2].x - x_coor;
    y_coor = beaconpos[2].y - y_coor;
#endif
#ifdef  TEAM_BLUE
    x_coor = x_coor - beaconpos[2].x;
    y_coor = y_coor - beaconpos[2].y;
#endif

    printf("solved pos from sonar: %f, %f \r\n", x_coor, y_coor);

    //IR
    float IRMeasuresloc[3];
    IRMeasuresloc[0] = RawReadings[IR0];
    IRMeasuresloc[1] = RawReadings[IR1];
    IRMeasuresloc[2] = RawReadings[IR2];
    printf("IR 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);

    float IR_Offsets[3] = {0};
    float frombrefoffset = 0;
    int refbeacon = 0;

    for (int i = 0; i < 3; i++) {
        if (sensorseenflags & 1<<(3+i)) {
            refbeacon = i;
            break;
        }
    }

    printf("refbeacon is %d\r\n", refbeacon);

    int cnt = 0;
    for (int i = 0; i < 3; i++) {

        if (sensorseenflags & 1<<(3+i)) {
            cnt++;

            //Compute IR offset
            float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);

            //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
            IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est);

            frombrefoffset += constrainAngle(IR_Offsets[i] - IR_Offsets[refbeacon]);
        }
    }
    
    printf("Used IR info from %d beacons\r\n", cnt);

    X(3,0) = constrainAngle(IR_Offsets[refbeacon] + frombrefoffset/cnt);

    //debug
    printf("Offsets IR: %0.4f\r\n",X(3,0)*180/PI);

    statelock.lock();
    X(0,0) = x_coor-TURRET_FWD_PLACEMENT; 
    X(1,0) = y_coor;
    X(2,0) = 0; //TODO: assume facing east, need to account for both starting pos

    P = 0.02*0.02,  0,          0,          0,
        0,          0.02*0.02,  0,          0,
        0,          0,          0.1*0.1,    -0.1*0.1,
        0,          0,          -0.1*0.1,   0.1*0.1 + 0.05*0.05;
        
    statelock.unlock();

    Kalman_inited = 1;
}


State getState()
{
    statelock.lock();
    State state = {X(0,0), X(1,0), X(2,0)};
    statelock.unlock();
    return state;
}


void predictloop(void const*)
{

    OLED4 = !Printing::registerID(0, 3) || OLED4;
    OLED4 = !Printing::registerID(1, 4) || OLED4;
    OLED4 = !Printing::registerID(8, 1) || OLED4;

    float lastleft = 0;
    float lastright = 0;

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

        float leftenc = left_encoder.getTicks() * ENCODER_M_PER_TICK;
        float rightenc = right_encoder.getTicks() * ENCODER_M_PER_TICK;

        float dleft = leftenc-lastleft;
        float dright = rightenc-lastright;

        lastleft = leftenc;
        lastright = rightenc;


        //The below calculation are in body frame (where +x is forward)
        float dxp, dyp,d,r;
        float thetap = (dright - dleft) / ENCODER_WHEELBASE;
        if (abs(thetap) < 0.01f) { //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 = r*sin(thetap);
            dyp = r - r*cos(thetap);
        }

        statelock.lock();

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

        //Linearising F around X
        Matrix<float, 4, 4> F;
        F = 1, 0, (dxp * -sin(tempX2) - dyp * cos(tempX2)), 0,
            0, 1, (dxp * cos(tempX2) - dyp * sin(tempX2)),  0,
            0, 0, 1,                                        0,
            0, 0, 0,                                        1;

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

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

        Qrot = Rotmatrix(X(2,0));

        Qsubrot = Qrot * Qsub * trans(Qrot);

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

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

        //printf("x: %f, y: %f, t: %f\r\n", X(0,0), X(1,0), X(2,0));
        //Update Printing
        float statecpy[] = {X(0,0), X(1,0), X(2,0)};
        Printing::updateval(0, statecpy, 3);
        
        Printing::updateval(8, X(3,0));

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

        statelock.unlock();
    }
}


void predict_event_setter()
{
    if(predict_thread_ptr)
        predict_thread_ptr->signal_set(0x1);
    else
        OLED4 = 1;
}

void start_predict_ticker(Thread* predict_thread_ptr_in)
{
    predict_thread_ptr = predict_thread_ptr_in;
    predictticker.attach(predict_event_setter, KALMAN_PREDICT_PERIOD);
}

void runupdate(measurement_t type, float value, float variance)
{
    sensorseenflags |= 1<<type;
    RawReadings[type] = value;

    if (Kalman_inited) {
        measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
        if (measured) {
            measured->mtype = type;
            measured->value = RawReadings[type];
            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(void const*)
{

    //sonar Y chanels
    OLED4 = !Printing::registerID(2, 1);
    OLED4 = !Printing::registerID(3, 1);
    OLED4 = !Printing::registerID(4, 1);

    //IR Y chanels
    OLED4 = !Printing::registerID(5, 1);
    OLED4 = !Printing::registerID(6, 1);
    OLED4 = !Printing::registerID(7, 1);

    bool aborton2stddev = false;

    Matrix<float, 1, 4> H;

    float Y,S;
    const Matrix<float, 4, 4> I4( identity< Matrix<float, 4, 4> >() );


    while (1) {
        OLED2 = !OLED2;

        osEvent evt = measureMQ.get();

        if (evt.status == osEventMail) {

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

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

            if (type <= maxmeasure) {

                if (type <= SONAR2) {

                    float dist = value;
                    int sonarid = type;
                    aborton2stddev = true;

                    statelock.lock();

                    float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0));
                    float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0));

                    float rbx = X(0,0) + fp_ct - beaconpos[sonarid].x;
                    float rby = X(1,0) + fp_st - beaconpos[sonarid].y;

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

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

                    float r_expecdist = 1.0f/expecdist;

                    float dhdx = rbx * r_expecdist;
                    float dhdy = rby * r_expecdist;
                    float dhdt = fp_ct*dhdy - fp_st*dhdx;

                    H = dhdx, dhdy, dhdt, 0;

                } else if (type <= IR2) {

                    aborton2stddev = true;
                    int IRidx = type-3;

                    statelock.lock();

                    float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0));
                    float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0));

                    float brx = beaconpos[IRidx].x - (X(0,0) + fp_ct);
                    float bry = beaconpos[IRidx].y - (X(1,0) + fp_st);

                    float expecang = atan2(bry, brx) - X(2,0) + X(3,0); //constrainAngle can be called late
                    Y = constrainAngle(value - expecang);

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

                    float r_dstsq = 1.0f/(brx*brx + bry*bry);
                    float dhdx = -bry*r_dstsq;
                    float dhdy = brx*r_dstsq;
                    float dhdt = fp_ct*dhdy - fp_st*dhdx - 1.0f;
                    float dhdp = 1;
                    H = dhdx, dhdy, dhdt, dhdp;
                }

                Matrix<float, 4, 1> PHt (P * trans(H));
                S = (H * PHt)(0,0) + variance*4; //TODO: Temp Hack!

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

                Matrix<float, 4, 1> K (PHt * (1/S));

                //Updating state
                X += K * Y;
                X(2,0) = constrainAngle(X(2,0));
                X(3,0) = constrainAngle(X(3,0));

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

                statelock.unlock();

            }

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

    }

}


} //Kalman Namespace