Diff: Processes/Kalman/Kalman.cpp

Revision:

16:52250d8d8fce

Child:

17:6263e90bf3ba

diff -r 9c5aaeda36dc -r 52250d8d8fce Processes/Kalman/Kalman.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Processes/Kalman/Kalman.cpp	Sun Apr 07 16:50:36 2013 +0000
@@ -0,0 +1,383 @@
+//***************************************************************************************
+//Kalman Filter implementation
+//***************************************************************************************
+#include "Kalman.h"
+#include "rtos.h"
+#include "math.h"
+#include "supportfuncs.h"
+//#include "globals.h"
+
+#include <tvmet/Matrix.h>
+using namespace tvmet;
+
+
+
+namespace Kalman
+{
+
+//State variables
+Vector<float, 3> X;
+Matrix<float, 3, 3> P;
+Mutex statelock;
+
+float RawReadings[maxmeasure+1];
+float SensorOffsets[maxmeasure+1] = {0};
+
+bool Kalman_init = 0;
+
+struct measurmentdata {
+    measurement_t mtype;
+    float value;
+    float variance;
+}
+
+Mail <measurmentdata, 16> measureMQ;
+
+
+
+//Note: this init function assumes that the robot faces east, theta=0, in the +x direction
+void KalmanInit()
+{
+    
+    //Solving for sonar bias is done by entering the following into wolfram alpha
+    //(a-f)^2 = x^2 + y^2; (b-f)^2 = (x-3)^2 + y^2; (c-f)^2 = (x-1.5)^2+(y-2)^2: solve for x,y,f
+    //where a, b, c are the measured distances, and f is the bias
+    
+    SensorOffsets[SONAR0] = sonartimebias;
+    SensorOffsets[SONAR1] = sonartimebias;
+    SensorOffsets[SONAR2] = sonartimebias;
+    
+    //solve for our position (assume perfect bias)
+    float x_coor = 
+    
+    
+    //IR
+
+    float IRMeasuresloc[3];
+
+    IRMeasuresloc[0] = RawReadings[IR0];
+    IRMeasuresloc[1] = RawReadings[IR1];
+    IRMeasuresloc[2] = RawReadings[IR2];
+    //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
+
+    IR_Offset = 0;
+    for (int i = 0; i < 3; i++) {
+
+        //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) {
+            // 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.lock();
+    X(0) = x_coor/1000.0f;
+    X(1) = y_coor/1000.0f;
+
+    if (Colour)
+        X(2) = 0;
+    else
+        X(2) = PI;
+    statelock.unlock();
+
+
+    //reattach the IR processing
+    ir.attachisr();
+}
+
+
+void Kalman::predictloop(void* dummy)
+{
+
+    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 = encoders.getEncoder1();
+        int rightenc = encoders.getEncoder2();
+
+        float dleft = encoders.encoderToDistance(leftenc-lastleft)/1000.0f;
+        float dright = encoders.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::runupdate(measurement_t type, float value, float variance)
+{
+    if (!Kalman_init)
+        RawReadings[type] = value;
+    else {
+        
+        RawReadings[type] = value - SensorOffsets[type];
+        
+        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(void* dummy)
+{
+
+    //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();
+
+}
+
+} //Kalman Namespace
+