Kalman.cpp

00001 //***************************************************************************************
00002 //Kalman Filter implementation
00003 //***************************************************************************************
00004 #include "Kalman.h"
00005 #include "rtos.h"
00006 #include "RFSRF05.h"
00007 #include "math.h"
00008 #include "globals.h"
00009 #include "motors.h"
00010 #include "system.h"
00011 #include "geometryfuncs.h"
00012 
00013 #include <tvmet/Matrix.h>
00014 #include <tvmet/Vector.h>
00015 using namespace tvmet;
00016 
00017 Kalman::Kalman(Motors &motorsin,
00018                UI &uiin,
00019                PinName Sonar_Trig,
00020                PinName Sonar_Echo0,
00021                PinName Sonar_Echo1,
00022                PinName Sonar_Echo2,
00023                PinName Sonar_Echo3,
00024                PinName Sonar_Echo4,
00025                PinName Sonar_Echo5,
00026                PinName Sonar_SDI,
00027                PinName Sonar_SDO,
00028                PinName Sonar_SCK,
00029                PinName Sonar_NCS,
00030                PinName Sonar_NIRQ,
00031                PinName IR_Tx,
00032                PinName IR_Rx) :
00033         ir(*this,IR_Tx,IR_Rx),
00034         sonararray(Sonar_Trig,
00035                    Sonar_Echo0,
00036                    Sonar_Echo1,
00037                    Sonar_Echo2,
00038                    Sonar_Echo3,
00039                    Sonar_Echo4,
00040                    Sonar_Echo5,
00041                    Sonar_SDI,
00042                    Sonar_SDO,
00043                    Sonar_SCK,
00044                    Sonar_NCS,
00045                    Sonar_NIRQ),
00046         motors(motorsin),
00047         ui(uiin),
00048         predictthread(predictloopwrapper, this, osPriorityNormal, 512),
00049         predictticker( SIGTICKARGS(predictthread, 0x1) ),
00050 //        sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
00051 //        sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
00052         updatethread(updateloopwrapper, this, osPriorityNormal, 512) {
00053 
00054     Kalman_init = false;
00055     //Intialising some arrays to zero
00056     for (int kk = 0; kk < 3; kk ++) {
00057         SonarMeasure_Offset[kk] = 0;
00058     }
00059     //Initialising other vars
00060 
00061 
00062     //Initilising matrices
00063 
00064     // X = x, y, theta;
00065     X = 0.5, 0, 0;
00066 
00067     P = 1, 0, 0,
00068         0, 1, 0,
00069         0, 0, 0.04;
00070 
00071     //measurment variance R is provided by each sensor when calling runupdate
00072 
00073     //attach callback
00074     sonararray.callbackobj = (DummyCT*)this;
00075     sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance, float variance)) &Kalman::runupdate;
00076 
00077 
00078     predictticker.start(20);
00079 //    sonarticker.start(50);
00080 
00081 }
00082 
00083 
00084 //Note: this init function assumes that the robot faces east, theta=0, in the +x direction
00085 void Kalman::KalmanInit() {
00086     float SonarMeasuresx1000[3];
00087     float IRMeasuresloc[3];
00088     int beacon_cnt = 0;
00089     // set initiating flag to false
00090     Kalman_init = false;
00091 
00092     // init the offset array
00093     for (int k = 0; k < 3; k ++) {
00094         SonarMeasure_Offset[k] = 0;
00095         IRMeasures[k] = 0;
00096     }
00097 
00098     LPC_UART0->FCR = LPC_UART0->FCR | 0x06;       // Flush the serial FIFO buffer / OR with FCR
00099     //wating untill the IR has reved up and picked up some data
00100     wait(1);
00101 
00102     //temporaraly disable IR updates
00103     ir.detachisr();
00104     //IRturret.attach(NULL,Serial::RxIrq);
00105 
00106     //lock the state throughout the computation, as we will override the state at the end
00107     statelock.lock();
00108 
00109     SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
00110     SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
00111     SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
00112     IRMeasuresloc[0] = IRMeasures[0];
00113     IRMeasuresloc[1] = IRMeasures[1];
00114     IRMeasuresloc[2] = IRMeasures[2];
00115     //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
00116 
00117     float d = beaconpos[2].y - beaconpos[1].y;
00118     float i = beaconpos[0].y - beaconpos[1].y;
00119     float j = beaconpos[0].x - beaconpos[1].x;
00120     float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
00121     float x_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
00122 
00123     //Compute sonar offset
00124     float Dist_Exp[3];
00125     for (int k = 0; k < 3; k++) {
00126         Dist_Exp[k] = sqrt((beaconpos[k].y - y_coor)*(beaconpos[k].y - y_coor)+(beaconpos[k].x - x_coor)*(beaconpos[k].x - x_coor));
00127         SonarMeasure_Offset[k] = (SonarMeasuresx1000[k]-Dist_Exp[k])/1000.0f;
00128     }
00129 
00130     //Compute IR offset
00131     ir.angleOffset = 0;
00132     for (int i = 0; i < 3; i++) {
00133         float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
00134         // take average offset angle from valid readings
00135         if (IRMeasuresloc[i] != 0) {
00136             beacon_cnt ++;
00137             // changed to current angle - estimated angle
00138             float angle_temp = IRMeasuresloc[i] - angle_est;
00139             angle_temp -= (floor(angle_temp/(2*PI)))*2*PI;
00140             ir.angleOffset += angle_temp;
00141         }
00142     }
00143     ir.angleOffset = ir.angleOffset/float(beacon_cnt);
00144     //printf("\n\r");
00145 
00146     //statelock already locked
00147     ir.angleInit = true;
00148     // set int flag to true
00149     Kalman_init = true;
00150     X(0) = x_coor/1000.0f;
00151     X(1) = y_coor/1000.0f;
00152     X(2) = 0;
00153     statelock.unlock();
00154 
00155     //printf("x: %0.4f, y: %0.4f, offset: %0.4f \n\r", x_coor, y_coor, angleOffset*180/PI);
00156 
00157     //reattach the IR processing
00158     ir.attachisr();
00159     //IRturret.attach(&IR::vIRValueISR,Serial::RxIrq);
00160 }
00161 
00162 
00163 void Kalman::predictloop() {
00164 
00165     float lastleft = 0;
00166     float lastright = 0;
00167 
00168     while (1) {
00169         Thread::signal_wait(0x1);
00170         OLED1 = !OLED1;
00171 
00172         int leftenc = motors.getEncoder1();
00173         int rightenc = motors.getEncoder2();
00174 
00175         float dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f;
00176         float dright = motors.encoderToDistance(rightenc-lastright)/1000.0f;
00177 
00178         lastleft = leftenc;
00179         lastright = rightenc;
00180 
00181 
00182         //The below calculation are in body frame (where +x is forward)
00183         float dxp, dyp,d,r;
00184         float thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f);
00185         if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
00186             d = (dright + dleft)/2.0f;
00187             dxp = d*cos(thetap/2.0f);
00188             dyp = d*sin(thetap/2.0f);
00189 
00190         } else { //calculate circle arc
00191             //float r = (right + left) / (4.0f * PI * thetap);
00192             r = (dright + dleft) / (2.0f*thetap);
00193             dxp = abs(r)*sin(thetap);
00194             dyp = r - r*cos(thetap);
00195         }
00196 
00197         statelock.lock();
00198 
00199         //rotating to cartesian frame and updating state
00200         X(0) += dxp * cos(X(2)) - dyp * sin(X(2));
00201         X(1) += dxp * sin(X(2)) + dyp * cos(X(2));
00202         X(2) = rectifyAng(X(2) + thetap);
00203 
00204         //Linearising F around X
00205         Matrix<float, 3, 3> F;
00206         F = 1, 0, (dxp * -sin(X(2)) - dyp * cos(X(2))),
00207             0, 1, (dxp * cos(X(2)) - dyp * sin(X(2))),
00208             0, 0, 1;
00209 
00210         //Generating forward and rotational variance
00211         float varfwd = fwdvarperunit * (dright + dleft) / 2.0f;
00212         float varang = varperang * thetap;
00213         float varxydt = xyvarpertime * PREDICTPERIOD;
00214         float varangdt = angvarpertime * PREDICTPERIOD;
00215 
00216         //Rotating into cartesian frame
00217         Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
00218         Qsub = varfwd + varxydt, 0,
00219                0, varxydt;
00220 
00221         Qrot = Rotmatrix(X(2));
00222 
00223         Qsubrot = Qrot * Qsub * trans(Qrot);
00224 
00225         //Generate Q
00226         Matrix<float, 3, 3> Q;//(Qsubrot);
00227         Q = Qsubrot(0,0), Qsubrot(0,1), 0,
00228             Qsubrot(1,0), Qsubrot(1,1), 0,
00229             0, 0, varang + varangdt;
00230 
00231         P = F * P * trans(F) + Q;
00232 
00233         statelock.unlock();
00234         //Thread::wait(PREDICTPERIOD);
00235 
00236         //cout << "predict" << X << endl;
00237         //cout << P << endl;
00238     }
00239 }
00240 
00241 //void Kalman::sonarloop() {
00242 //    while (1) {
00243 //        Thread::signal_wait(0x1);
00244 //        sonararray.startRange();
00245 //    }
00246 //}
00247 
00248 
00249 void Kalman::runupdate(measurement_t type, float value, float variance) {
00250     //printf("beacon %d dist %f\r\n", sonarid, dist);
00251     //led2 = !led2;
00252 
00253     measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
00254     if (measured) {
00255         measured->mtype = type;
00256         measured->value = value;
00257         measured->variance = variance;
00258 
00259         osStatus putret = measureMQ.put(measured);
00260         if (putret)
00261             OLED4 = 1;
00262         //    printf("putting in MQ error code %#x\r\n", putret);
00263     } else {
00264         OLED4 = 1;
00265         //printf("MQalloc returned NULL ptr\r\n");
00266     }
00267 
00268 }
00269 
00270 void Kalman::updateloop() {
00271     measurement_t type;
00272     float value,variance,rbx,rby,expecdist,Y;
00273     float dhdx,dhdy;
00274     bool aborton2stddev = false;
00275 
00276     Matrix<float, 1, 3> H;
00277 
00278     float S;
00279     Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
00280 
00281 
00282     while (1) {
00283         OLED2 = !OLED2;
00284 
00285         osEvent evt = measureMQ.get();
00286 
00287         if (evt.status == osEventMail) {
00288 
00289             measurmentdata &measured = *(measurmentdata*)evt.value.p;
00290             type = measured.mtype; //Note, may support more measurment types than sonar in the future!
00291             value = measured.value;
00292             variance = measured.variance;
00293 
00294             // don't forget to free the memory
00295             measureMQ.free(&measured);
00296 
00297             if (type <= maxmeasure) {
00298 
00299                 if (type <= SONAR3) {
00300 
00301                     float dist = value / 1000.0f; //converting to m from mm
00302                     int sonarid = type;
00303                     aborton2stddev = false;
00304 
00305                     // Remove the offset if possible
00306                     if (Kalman_init)
00307                         dist = dist - SonarMeasure_Offset[sonarid];
00308 
00309                     statelock.lock();
00310                     //update the current sonar readings
00311                     SonarMeasures[sonarid] = dist;
00312 
00313                     rbx = X(0) - beaconpos[sonarid].x/1000.0f;
00314                     rby = X(1) - beaconpos[sonarid].y/1000.0f;
00315 
00316                     expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
00317                     Y = dist - expecdist;
00318 
00319                     dhdx = rbx / expecdist;
00320                     dhdy = rby / expecdist;
00321 
00322                     H = dhdx, dhdy, 0;
00323 
00324                 } else if (type <= IR3) {
00325 
00326                     aborton2stddev = false;
00327                     int IRidx = type-3;
00328 
00329                     statelock.lock();
00330                     IRMeasures[IRidx] = value;
00331 
00332                     rbx = X(0) - beaconpos[IRidx].x/1000.0f;
00333                     rby = X(1) - beaconpos[IRidx].y/1000.0f;
00334 
00335                     float expecang = atan2(-rby, -rbx) - X(2);
00336                     Y = rectifyAng(value - expecang);
00337 
00338                     float dstsq = rbx*rbx + rby*rby;
00339                     H = -rby/dstsq, rbx/dstsq, -1;
00340                 }
00341 
00342                 Matrix<float, 3, 1> PH (P * trans(H));
00343                 S = (H * PH)(0,0) + variance;
00344 
00345                 if (aborton2stddev && Y*Y > 4 * S) {
00346                     statelock.unlock();
00347                     continue;
00348                 }
00349 
00350                 Matrix<float, 3, 1> K (PH * (1/S));
00351 
00352                 //Updating state
00353                 X += col(K, 0) * Y;
00354                 X(2) = rectifyAng(X(2));
00355 
00356                 P = (I3 - K * H) * P;
00357 
00358                 statelock.unlock();
00359 
00360             }
00361 
00362         } else {
00363             OLED4 = 1;
00364             //printf("ERROR: in updateloop, code %#x", evt);
00365         }
00366 
00367     }
00368 
00369 }