Kalman.cpp

00001 //***************************************************************************************
00002 //Kalman Filter implementation
00003 //***************************************************************************************
00004 #include "Kalman.h"
00005 #include "rtos.h"
00006 #include "RFSRF05.h"
00007 //#include "MatrixMath.h"
00008 //#include "Matrix.h"
00009 #include "math.h"
00010 #include "globals.h"
00011 #include "motors.h"
00012 #include "system.h"
00013 #include "geometryfuncs.h"
00014 
00015 #include <tvmet/Matrix.h>
00016 #include <tvmet/Vector.h>
00017 using namespace tvmet;
00018 DigitalOut led1(LED1);
00019 DigitalOut led2(LED2);
00020 DigitalOut led3(LED3);
00021 DigitalOut led4(LED4);
00022 
00023 
00024 Kalman::Kalman(Motors &motorsin) :
00025         sonararray(p10,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9),
00026         motors(motorsin),
00027         predictthread(predictloopwrapper, this, osPriorityNormal, 512),
00028         predictticker( SIGTICKARGS(predictthread, 0x1) ),
00029 //        sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
00030 //        sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
00031         updatethread(updateloopwrapper, this, osPriorityNormal, 2048) {
00032 
00033     //Initilising matrices
00034 
00035     // X = x, y, theta;
00036     X = 0.5, 0, 0;
00037 
00038     P = 1, 0, 0,
00039         0, 1, 0,
00040         0, 0, 0.04;
00041 
00042     //Q = 0.002, 0, 0, //temporary matrix, Use dt!
00043     //    0, 0.002, 0,
00044     //    0, 0, 0.002;
00045 
00046     //measurment variance R is provided by each sensor when calling runupdate
00047 
00048     //attach callback
00049     sonararray.callbackobj = (DummyCT*)this;
00050     sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance, float variance)) &Kalman::runupdate;
00051 
00052 
00053     predictticker.start(20);
00054 //    sonarticker.start(50);
00055 
00056 
00057 }
00058 
00059 
00060 void Kalman::predictloop() {
00061 
00062     float lastleft = 0;
00063     float lastright = 0;
00064 
00065     while (1) {
00066         Thread::signal_wait(0x1);
00067         led1 = !led1;
00068         
00069         int leftenc = motors.getEncoder1();
00070         int rightenc = motors.getEncoder2();
00071         
00072         float dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f;
00073         float dright = motors.encoderToDistance(rightenc-lastright)/1000.0f;
00074 
00075         lastleft = leftenc;
00076         lastright = rightenc;
00077 
00078 
00079         //The below calculation are in body frame (where +x is forward)
00080         float dxp, dyp,d,r;
00081         float thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f);
00082         if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
00083             d = (dright + dleft)/2.0f;
00084             dxp = d*cos(thetap/2.0f);
00085             dyp = d*sin(thetap/2.0f);
00086 
00087         } else { //calculate circle arc
00088             //float r = (right + left) / (4.0f * PI * thetap);
00089             r = (dright + dleft) / (2.0f*thetap);
00090             dxp = abs(r)*sin(thetap);
00091             dyp = r - r*cos(thetap);
00092         }
00093 
00094         statelock.lock();
00095 
00096         //rotating to cartesian frame and updating state
00097         X(0) += dxp * cos(X(2)) - dyp * sin(X(2));
00098         X(1) += dxp * sin(X(2)) + dyp * cos(X(2));
00099         X(2) = rectifyAng(X(2) + thetap);
00100 
00101         //Linearising F around X
00102         Matrix<float, 3, 3> F;
00103         F = 1, 0, (dxp * -sin(X(2)) - dyp * cos(X(2))),
00104             0, 1, (dxp * cos(X(2)) - dyp * sin(X(2))),
00105             0, 0, 1;
00106 
00107         //Generating forward and rotational variance
00108         float varfwd = fwdvarperunit * (dright + dleft) / 2.0f;
00109         float varang = varperang * thetap;
00110         float varxydt = xyvarpertime * PREDICTPERIOD;
00111         float varangdt = angvarpertime * PREDICTPERIOD;
00112         
00113         //Rotating into cartesian frame
00114         Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
00115         Qsub = varfwd + varxydt, 0,
00116                0, varxydt;
00117                
00118         Qrot = Rotmatrix(X(2));
00119                
00120         Qsubrot = Qrot * Qsub * trans(Qrot);
00121 
00122         //Generate Q
00123         Matrix<float, 3, 3> Q;//(Qsubrot);
00124         Q = Qsubrot(0,0), Qsubrot(0,1), 0,
00125             Qsubrot(1,0), Qsubrot(1,1), 0,
00126             0, 0, varang + varangdt;
00127 
00128         P = F * P * trans(F) + Q;
00129 
00130         statelock.unlock();
00131         //Thread::wait(PREDICTPERIOD);
00132 
00133         //cout << "predict" << X << endl;
00134         //cout << P << endl;
00135     }
00136 }
00137 
00138 //void Kalman::sonarloop() {
00139 //    while (1) {
00140 //        Thread::signal_wait(0x1);
00141 //        sonararray.startRange();
00142 //    }
00143 //}
00144 
00145 
00146 void Kalman::runupdate(measurement_t type, float value, float variance) {
00147     //printf("beacon %d dist %f\r\n", sonarid, dist);
00148     //led2 = !led2;
00149 
00150     measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
00151     if (measured) {
00152         measured->mtype = type;
00153         measured->value = value;
00154         measured->variance = variance;
00155 
00156         osStatus putret = measureMQ.put(measured);
00157         if (putret)
00158             led4 = 1;
00159         //    printf("putting in MQ error code %#x\r\n", putret);
00160     } else {
00161         led4 = 1;
00162         //printf("MQalloc returned NULL ptr\r\n");
00163     }
00164 
00165 }
00166 
00167 void Kalman::updateloop() {
00168     measurement_t type;
00169     float value,variance,rbx,rby,expecdist,Y;
00170     float dhdx,dhdy;
00171     bool aborton2stddev = false;
00172 
00173     Matrix<float, 1, 3> H;
00174 
00175     float S;
00176     Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
00177 
00178 
00179     while (1) {
00180         led2 = !led2;
00181 
00182         osEvent evt = measureMQ.get();
00183 
00184         if (evt.status == osEventMail) {
00185 
00186             measurmentdata &measured = *(measurmentdata*)evt.value.p;
00187             type = measured.mtype; //Note, may support more measurment types than sonar in the future!
00188             value = measured.value;
00189             variance = measured.variance;
00190 
00191             // don't forget to free the memory
00192             measureMQ.free(&measured);
00193 
00194             if (type <= maxmeasure) {
00195 
00196                 if (type <= SONAR3) {
00197 
00198                     float dist = value / 1000.0f; //converting to m from mm
00199                     int sonarid = type;
00200                     aborton2stddev = false; 
00201 
00202                     statelock.lock();
00203                     SonarMeasures[sonarid] = dist; //update the current sonar readings
00204 
00205                     rbx = X(0) - beaconpos[sonarid].x/1000.0f;
00206                     rby = X(1) - beaconpos[sonarid].y/1000.0f;
00207 
00208                     expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
00209                     Y = dist - expecdist;
00210 
00211                     dhdx = rbx / expecdist;
00212                     dhdy = rby / expecdist;
00213 
00214                     H = dhdx, dhdy, 0;
00215 
00216                 } else if (type <= IR3) {
00217                   
00218                     aborton2stddev = false;
00219                     int IRidx = type-3;
00220                     
00221                     statelock.lock();
00222                     IRMeasures[IRidx] = value;
00223                     
00224                     rbx = X(0) - beaconpos[IRidx].x/1000.0f;
00225                     rby = X(1) - beaconpos[IRidx].y/1000.0f;
00226                     
00227                     float expecang = atan2(-rbx, -rby) - X(2);
00228                     //printf("expecang: %0.4f, value: %0.4f \n\r", expecang*180/PI,value*180/PI);
00229                     Y = rectifyAng(value + expecang);
00230                     
00231                     float dstsq = rbx*rbx + rby*rby;
00232                     H = -rby/dstsq, rbx/dstsq, -1;
00233                 }
00234 
00235                 Matrix<float, 3, 1> PH (P * trans(H));
00236                 S = (H * PH)(0,0) + variance;
00237 
00238                 if (aborton2stddev && Y*Y > 4 * S) {
00239                     statelock.unlock();
00240                     continue;
00241                 }
00242 
00243                 Matrix<float, 3, 1> K (PH * (1/S));
00244 
00245                 //Updating state
00246                 X += col(K, 0) * Y;
00247                 X(2) = rectifyAng(X(2));
00248 
00249                 P = (I3 - K * H) * P;
00250 
00251                 statelock.unlock();
00252 
00253             }
00254 
00255         } else {
00256             led4 = 1;
00257             //printf("ERROR: in updateloop, code %#x", evt);
00258         }
00259 
00260     }
00261 
00262 }