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 "encoders.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(Encoders &encodersin,
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         ir(*this),
00032         sonararray(Sonar_Trig,
00033                    Sonar_Echo0,
00034                    Sonar_Echo1,
00035                    Sonar_Echo2,
00036                    Sonar_Echo3,
00037                    Sonar_Echo4,
00038                    Sonar_Echo5,
00039                    Sonar_SDI,
00040                    Sonar_SDO,
00041                    Sonar_SCK,
00042                    Sonar_NCS,
00043                    Sonar_NIRQ),
00044         encoders(encodersin),
00045         ui(uiin),
00046         predictthread(predictloopwrapper, this, osPriorityNormal, 512),
00047         predictticker( SIGTICKARGS(predictthread, 0x1) ),
00048 //        sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
00049 //        sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
00050         updatethread(updateloopwrapper, this, osPriorityNormal, 512) {
00051 
00052     //Initilising offsets
00053     InitLock.lock();
00054     IR_Offset = 0;
00055     Sonar_Offset = 0;
00056     InitLock.unlock();
00057 
00058 
00059     //Initilising matrices
00060 
00061     // X = x, y, theta;
00062     if (Colour)
00063         X = 0.5, 0, 0;
00064     else
00065         X = 2.5, 0, PI;
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 
00087     float SonarMeasuresx1000[3];
00088     float IRMeasuresloc[3];
00089     int beacon_cnt = 0;
00090 
00091 
00092 // doesn't work since they break the ISR
00093     /*
00094     #ifdef ROBOT_PRIMARY
00095         LPC_UART3->FCR = LPC_UART3->FCR | 0x06;       // Flush the serial FIFO buffer / OR with FCR
00096     #else
00097         LPC_UART1->FCR = LPC_UART1->FCR | 0x06;       // Flush the serial FIFO buffer / OR with FCR
00098     #endif
00099     */
00100     // zeros the measurements
00101     for (int i = 0; i < 3; i++) {
00102         SonarMeasures[i] = 0;
00103         IRMeasures[i] = 0;
00104     }
00105 
00106     InitLock.lock();
00107     //zeros offsets
00108     IR_Offset = 0;
00109     Sonar_Offset = 0;
00110     InitLock.unlock();
00111 
00112     // attaches ir interrup
00113     ir.attachisr();
00114 
00115     //wating untill the IR has reved up and picked up some valid data
00116     //Thread::wait(1000);
00117     wait(2);
00118 
00119     //temporaraly disable IR updates
00120     ir.detachisr();
00121 
00122     //lock the state throughout the computation, as we will override the state at the end
00123     InitLock.lock();
00124     statelock.lock();
00125 
00126 
00127 
00128     SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
00129     SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
00130     SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
00131     IRMeasuresloc[0] = IRMeasures[0];
00132     IRMeasuresloc[1] = IRMeasures[1];
00133     IRMeasuresloc[2] = IRMeasures[2];
00134     //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
00135 
00136     float d = beaconpos[2].y - beaconpos[1].y;
00137     float i = beaconpos[0].y - beaconpos[1].y;
00138     float j = beaconpos[0].x - beaconpos[1].x;
00139     float origin_x = beaconpos[1].x;
00140     float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
00141     float x_coor = origin_x + (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
00142 
00143     //debug for trilateration
00144     printf("Cal at x: %0.4f, y: %0.4f \r\n",x_coor,y_coor );
00145 
00146     float Dist_Exp[3];
00147     for (int i = 0; i < 3; i++) {
00148         //Compute sonar offset
00149         Dist_Exp[i] = hypot(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
00150         Sonar_Offset += (SonarMeasuresx1000[i]-Dist_Exp[i])/3000.0f;
00151 
00152         //Compute IR offset
00153         float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
00154         if (!Colour)
00155         angle_est -= PI;
00156         //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
00157         // take average offset angle from valid readings
00158         if (IRMeasuresloc[i] != 0) {
00159             beacon_cnt ++;
00160             // changed to current angle - estimated angle
00161             float angle_temp = IRMeasuresloc[i] - angle_est;
00162             angle_temp -= (floor(angle_temp/(2*PI)))*2*PI;
00163             IR_Offset += angle_temp;
00164         }
00165     }
00166     IR_Offset /= float(beacon_cnt);
00167 
00168     //debug
00169     printf("Offsets IR: %0.4f, Sonar: %0.4f \r\n",IR_Offset*180/PI,Sonar_Offset*1000 );
00170 
00171     //statelock already locked
00172     X(0) = x_coor/1000.0f;
00173     X(1) = y_coor/1000.0f;
00174     
00175     if (Colour)
00176         X(2) = 0;
00177     else
00178         X(2) = PI;
00179 
00180     // unlocks mutexes
00181     InitLock.unlock();
00182     statelock.unlock();
00183 
00184 
00185     //reattach the IR processing
00186     ir.attachisr();
00187 }
00188 
00189 
00190 void Kalman::predictloop() {
00191 
00192     OLED4 = !ui.regid(0, 3);
00193     OLED4 = !ui.regid(1, 4);
00194 
00195     float lastleft = 0;
00196     float lastright = 0;
00197 
00198     while (1) {
00199         Thread::signal_wait(0x1);
00200         OLED1 = !OLED1;
00201 
00202         int leftenc = encoders.getEncoder1();
00203         int rightenc = encoders.getEncoder2();
00204 
00205         float dleft = encoders.encoderToDistance(leftenc-lastleft)/1000.0f;
00206         float dright = encoders.encoderToDistance(rightenc-lastright)/1000.0f;
00207 
00208         lastleft = leftenc;
00209         lastright = rightenc;
00210 
00211 
00212         //The below calculation are in body frame (where +x is forward)
00213         float dxp, dyp,d,r;
00214         float thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f);
00215         if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
00216             d = (dright + dleft)/2.0f;
00217             dxp = d*cos(thetap/2.0f);
00218             dyp = d*sin(thetap/2.0f);
00219 
00220         } else { //calculate circle arc
00221             //float r = (right + left) / (4.0f * PI * thetap);
00222             r = (dright + dleft) / (2.0f*thetap);
00223             dxp = abs(r)*sin(thetap);
00224             dyp = r - r*cos(thetap);
00225         }
00226 
00227         statelock.lock();
00228 
00229         float tempX2 = X(2);
00230         //rotating to cartesian frame and updating state
00231         X(0) += dxp * cos(X(2)) - dyp * sin(X(2));
00232         X(1) += dxp * sin(X(2)) + dyp * cos(X(2));
00233         X(2) = rectifyAng(X(2) + thetap);
00234 
00235         //Linearising F around X
00236         float avgX2 = (X(2) + tempX2)/2.0f;
00237         Matrix<float, 3, 3>  F;
00238         F = 1, 0, (dxp * -sin(avgX2) - dyp * cos(avgX2)),
00239             0, 1, (dxp * cos(avgX2) - dyp * sin(avgX2)),
00240             0, 0, 1;
00241 
00242         //Generating forward and rotational variance
00243         float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f;
00244         float varang = varperang * abs(thetap);
00245         float varxydt = xyvarpertime * PREDICTPERIOD/1000.0f;
00246         float varangdt = angvarpertime * PREDICTPERIOD/1000.0f;
00247 
00248         //Rotating into cartesian frame
00249         Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
00250         Qsub = varfwd + varxydt, 0,
00251                0, varxydt;
00252 
00253         Qrot = Rotmatrix(X(2));
00254 
00255         Qsubrot = Qrot * Qsub * trans(Qrot);
00256 
00257         //Generate Q
00258         Matrix<float, 3, 3>  Q;//(Qsubrot);
00259         Q = Qsubrot(0,0), Qsubrot(0,1), 0,
00260             Qsubrot(1,0), Qsubrot(1,1), 0,
00261             0, 0, varang + varangdt;
00262 
00263         P = F * P * trans(F) + Q;
00264 
00265         //Update UI
00266         float statecpy[] = {X(0), X(1), X(2)};
00267         ui.updateval(0, statecpy, 3);
00268 
00269         float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
00270         ui.updateval(1, Pcpy, 4);
00271 
00272         statelock.unlock();
00273     }
00274 }
00275 
00276 //void Kalman::sonarloop() {
00277 //    while (1) {
00278 //        Thread::signal_wait(0x1);
00279 //        sonararray.startRange();
00280 //    }
00281 //}
00282 
00283 
00284 void Kalman::runupdate(measurement_t type, float value, float variance) {
00285     //printf("beacon %d dist %f\r\n", sonarid, dist);
00286     //led2 = !led2;
00287 
00288     measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
00289     if (measured) {
00290         measured->mtype = type;
00291         measured->value = value;
00292         measured->variance = variance;
00293 
00294         osStatus putret = measureMQ.put(measured);
00295         if (putret)
00296             OLED4 = 1;
00297         //    printf("putting in MQ error code %#x\r\n", putret);
00298     } else {
00299         OLED4 = 1;
00300         //printf("MQalloc returned NULL ptr\r\n");
00301     }
00302 
00303 }
00304 
00305 void Kalman::updateloop() {
00306 
00307     //sonar Y chanels
00308     ui.regid(2, 1);
00309     ui.regid(3, 1);
00310     ui.regid(4, 1);
00311 
00312     //IR Y chanels
00313     ui.regid(5, 1);
00314     ui.regid(6, 1);
00315     ui.regid(7, 1);
00316 
00317     measurement_t type;
00318     float value,variance,rbx,rby,expecdist,Y;
00319     float dhdx,dhdy;
00320     bool aborton2stddev = false;
00321 
00322     Matrix<float, 1, 3> H;
00323 
00324     float S;
00325     Matrix<float, 3, 3>  I3( identity< Matrix<float, 3, 3>  >() );
00326 
00327 
00328     while (1) {
00329         OLED2 = !OLED2;
00330 
00331         osEvent evt = measureMQ.get();
00332 
00333         if (evt.status == osEventMail) {
00334 
00335             measurmentdata &measured = *(measurmentdata*)evt.value.p;
00336             type = measured.mtype; //Note, may support more measurment types than sonar in the future!
00337             value = measured.value;
00338             variance = measured.variance;
00339 
00340             // don't forget to free the memory
00341             measureMQ.free(&measured);
00342 
00343             if (type <= maxmeasure) {
00344 
00345                 if (type <= SONAR3) {
00346 
00347                     InitLock.lock();
00348                     float dist = value / 1000.0f - Sonar_Offset; //converting to m from mm,subtract the offset
00349                     InitLock.unlock();
00350 
00351                     int sonarid = type;
00352                     aborton2stddev = true;
00353 
00354                     statelock.lock();
00355                     //update the current sonar readings
00356                     SonarMeasures[sonarid] = dist;
00357 
00358                     rbx = X(0) - beaconpos[sonarid].x/1000.0f;
00359                     rby = X(1) - beaconpos[sonarid].y/1000.0f;
00360 
00361                     expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
00362                     Y = dist - expecdist;
00363 
00364                     //send to ui
00365                     ui.updateval(sonarid+2, Y);
00366 
00367                     dhdx = rbx / expecdist;
00368                     dhdy = rby / expecdist;
00369 
00370                     H = dhdx, dhdy, 0;
00371 
00372                 } else if (type <= IR3) {
00373 
00374                     aborton2stddev = false;
00375                     int IRidx = type-3;
00376 
00377                     // subtract the IR offset
00378                     InitLock.lock();
00379                     value -= IR_Offset;
00380                     InitLock.unlock();
00381 
00382                     statelock.lock();
00383                     IRMeasures[IRidx] = value;
00384 
00385                     rbx = X(0) - beaconpos[IRidx].x/1000.0f;
00386                     rby = X(1) - beaconpos[IRidx].y/1000.0f;
00387 
00388                     float expecang = atan2(-rby, -rbx) - X(2);
00389                     Y = rectifyAng(value - expecang);
00390 
00391                     //send to ui
00392                     ui.updateval(IRidx + 5, Y);
00393 
00394                     float dstsq = rbx*rbx + rby*rby;
00395                     H = -rby/dstsq, rbx/dstsq, -1;
00396                 }
00397 
00398                 Matrix<float, 3, 1> PH (P * trans(H));
00399                 S = (H * PH)(0,0) + variance;
00400 
00401                 if (aborton2stddev && Y*Y > 4 * S) {
00402                     statelock.unlock();
00403                     continue;
00404                 }
00405 
00406                 Matrix<float, 3, 1> K (PH * (1/S));
00407 
00408                 //Updating state
00409                 X += col(K, 0) * Y;
00410                 X(2) = rectifyAng(X(2));
00411 
00412                 P = (I3 - K * H) * P;
00413 
00414                 statelock.unlock();
00415 
00416             }
00417 
00418         } else {
00419             OLED4 = 1;
00420             //printf("ERROR: in updateloop, code %#x", evt);
00421         }
00422 
00423     }
00424 
00425 }
00426 
00427 // reset kalman states
00428 void Kalman::KalmanReset() {
00429     float SonarMeasuresx1000[3];
00430     statelock.lock();
00431     SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
00432     SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
00433     SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
00434     //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
00435 
00436     float d = beaconpos[2].y - beaconpos[1].y;
00437     float i = beaconpos[0].y - beaconpos[1].y;
00438     float j = beaconpos[0].x - beaconpos[1].x;
00439     float origin_x = beaconpos[1].x;
00440     float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
00441     float x_coor = origin_x +(SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
00442 
00443     //statelock already locked
00444     X(0) = x_coor/1000.0f;
00445     X(1) = y_coor/1000.0f;
00446    
00447     
00448 
00449 /*    if (Colour){
00450         X(0) = 0.2;
00451         X(1) = 0.2;
00452         //X(2) = 0;
00453         }
00454     else {
00455         X(0) = 2.8;
00456         X(1) = 0.2;
00457         //X(2) = PI;
00458     }
00459     */
00460     P = 0.05, 0, 0,
00461         0, 0.05, 0,
00462         0, 0, 0.04;
00463 
00464     // unlocks mutexes
00465     statelock.unlock();
00466 
00467 }