Kalman.cpp

00001 //***************************************************************************************
00002 //Kalman Filter implementation
00003 //***************************************************************************************
00004 #include "Kalman.h"
00005 #include "rtos.h"
00006 #include "math.h"
00007 #include "supportfuncs.h"
00008 #include "Encoder.h"
00009 #include "globals.h"
00010 #include "Printing.h"
00011 
00012 #include "tvmet/Matrix.h"
00013 using namespace tvmet;
00014 
00015 
00016 
00017 namespace Kalman
00018 {
00019 
00020 Ticker predictticker;
00021 
00022 DigitalOut OLED4(LED4);
00023 DigitalOut OLED3(LED3);
00024 DigitalOut OLED1(LED1);
00025 DigitalOut OLED2(LED2);
00026 
00027 //State variables
00028 Matrix<float, 4, 1> X;
00029 Matrix<float, 4, 4> P;
00030 Mutex statelock;
00031 
00032 float RawReadings[maxmeasure+1];
00033 volatile int sensorseenflags = 0;
00034 
00035 bool Kalman_inited = 0;
00036 
00037 struct measurmentdata {
00038     measurement_t mtype;
00039     float value;
00040     float variance;
00041 };
00042 
00043 Mail <measurmentdata, 16> measureMQ;
00044 
00045 Thread* predict_thread_ptr = NULL;
00046 
00047 
00048 //Note: this init function assumes that the robot faces east, theta=0, in the +x direction
00049 void KalmanInit()
00050 {
00051     printf("kalmaninit \r\n");
00052 
00053     //WARNING: HARDCODED! TODO: fix it so it works for both sides!
00054 
00055     printf("waiting for all sonar, and at least 1 IR\r\n");
00056     while( ((sensorseenflags & 0x7)^0x7) || !(sensorseenflags & 0x7<<3) );
00057 
00058 //#ifdef TEAM_RED
00059     //solve for our position (assume perfect bias)
00060     const float d = beaconpos[2].y - beaconpos[1].y;
00061     const float i = beaconpos[2].y - beaconpos[0].y;
00062     const float j = beaconpos[2].x - beaconpos[0].x;
00063     float r1 = RawReadings[SONAR2];
00064     float r2 = RawReadings[SONAR1];
00065     float r3 = RawReadings[SONAR0];
00066 //#endif
00067 /*
00068 #ifdef  TEAM_BLUE
00069     const float d = beaconpos[1].y - beaconpos[2].y;
00070     const float i = beaconpos[0].y - beaconpos[2].y;
00071     const float j = beaconpos[0].x - beaconpos[2].x;
00072     float r1 = RawReadings[SONAR2];
00073     float r2 = RawReadings[SONAR1];
00074     float r3 = RawReadings[SONAR0];
00075 #endif
00076 */
00077 
00078     printf("ranges: 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", r1, r2, r3);
00079 
00080     float y_coor = (r1*r1-r2*r2+d*d)/(2*d);
00081     float x_coor = (r1*r1-r3*r3+i*i+j*j)/(2*j) - (i*y_coor)/j;
00082 
00083 //#ifdef TEAM_RED
00084     //coordinate system hack (for now)
00085     x_coor = beaconpos[2].x - x_coor;
00086     y_coor = beaconpos[2].y - y_coor;
00087 //#endif
00088 /*
00089 #ifdef  TEAM_BLUE
00090     x_coor = x_coor - beaconpos[2].x;
00091     y_coor = y_coor - beaconpos[2].y;
00092 #endif
00093 */
00094 
00095     printf("solved pos from sonar: %f, %f \r\n", x_coor, y_coor);
00096 
00097     //IR
00098     float IRMeasuresloc[3];
00099     IRMeasuresloc[0] = RawReadings[IR0];
00100     IRMeasuresloc[1] = RawReadings[IR1];
00101     IRMeasuresloc[2] = RawReadings[IR2];
00102     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);
00103 
00104     float IR_Offsets[3] = {0};
00105     float frombrefoffset = 0;
00106     int refbeacon = 0;
00107 
00108     for (int i = 0; i < 3; i++) {
00109         if (sensorseenflags & 1<<(3+i)) {
00110             refbeacon = i;
00111             break;
00112         }
00113     }
00114 
00115     printf("refbeacon is %d\r\n", refbeacon);
00116 
00117     int cnt = 0;
00118     for (int i = 0; i < 3; i++) {
00119 
00120         if (sensorseenflags & 1<<(3+i)) {
00121             cnt++;
00122 
00123             //Compute IR offset
00124             float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
00125 
00126             //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
00127             IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
00128 
00129             frombrefoffset += constrainAngle(IR_Offsets[i] - IR_Offsets[refbeacon]);
00130         }
00131     }
00132     
00133     printf("Used IR info from %d beacons\r\n", cnt);
00134 
00135     X(3,0) = constrainAngle(IR_Offsets[refbeacon] + frombrefoffset/cnt);
00136 
00137     //debug
00138     printf("Offsets IR: %0.4f\r\n",X(3,0)*180/PI);
00139 
00140     statelock.lock();
00141     X(0,0) = x_coor-TURRET_FWD_PLACEMENT; 
00142     X(1,0) = y_coor;
00143     X(2,0) = 0; //TODO: assume facing east, need to account for both starting pos
00144 
00145     P = 0.02*0.02,  0,          0,          0,
00146         0,          0.02*0.02,  0,          0,
00147         0,          0,          0.1*0.1,    -0.1*0.1,
00148         0,          0,          -0.1*0.1,   0.1*0.1 + 0.05*0.05;
00149         
00150     statelock.unlock();
00151 
00152     Kalman_inited = 1;
00153 }
00154 
00155 
00156 State getState()
00157 {
00158     statelock.lock();
00159     State state = {X(0,0), X(1,0), X(2,0)};
00160     statelock.unlock();
00161     return state;
00162 }
00163 
00164 
00165 void predictloop(void const*)
00166 {
00167 
00168     OLED4 = !Printing::registerID(0, 3) || OLED4;
00169     OLED4 = !Printing::registerID(1, 4) || OLED4;
00170     OLED4 = !Printing::registerID(8, 1) || OLED4;
00171 
00172     float lastleft = 0;
00173     float lastright = 0;
00174 
00175     while (1) {
00176         Thread::signal_wait(0x1);
00177         OLED1 = !OLED1;
00178 
00179         float leftenc = left_encoder.getTicks() * ENCODER_M_PER_TICK;
00180         float rightenc = right_encoder.getTicks() * ENCODER_M_PER_TICK;
00181 
00182         float dleft = leftenc-lastleft;
00183         float dright = rightenc-lastright;
00184 
00185         lastleft = leftenc;
00186         lastright = rightenc;
00187 
00188 
00189         //The below calculation are in body frame (where +x is forward)
00190         float dxp, dyp,d,r;
00191         float thetap = (dright - dleft) / ENCODER_WHEELBASE;
00192         if (abs(thetap) < 0.01f) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
00193             d = (dright + dleft)/2.0f;
00194             dxp = d*cos(thetap/2.0f);
00195             dyp = d*sin(thetap/2.0f);
00196 
00197         } else { //calculate circle arc
00198             //float r = (right + left) / (4.0f * PI * thetap);
00199             r = (dright + dleft) / (2.0f*thetap);
00200             dxp = r*sin(thetap);
00201             dyp = r - r*cos(thetap);
00202         }
00203 
00204         statelock.lock();
00205 
00206         float tempX2 = X(2,0);
00207         //rotating to cartesian frame and updating state
00208         X(0,0) += dxp * cos(X(2,0)) - dyp * sin(X(2,0));
00209         X(1,0) += dxp * sin(X(2,0)) + dyp * cos(X(2,0));
00210         X(2,0) = constrainAngle(X(2,0) + thetap);
00211         //X(3,0) += 0;
00212 
00213         //Linearising F around X
00214         Matrix<float, 4, 4> F;
00215         F = 1, 0, (dxp * -sin(tempX2) - dyp * cos(tempX2)), 0,
00216             0, 1, (dxp * cos(tempX2) - dyp * sin(tempX2)),  0,
00217             0, 0, 1,                                        0,
00218             0, 0, 0,                                        1;
00219 
00220         //Generating forward and rotational variance
00221         float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f;
00222         float varang = varperang * abs(thetap);
00223         float varxydt = xyvarpertime * KALMAN_PREDICT_PERIOD;
00224         float varangdt = angvarpertime * KALMAN_PREDICT_PERIOD;
00225 
00226         //Rotating into cartesian frame
00227         Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
00228         Qsub = varfwd + varxydt, 0,
00229         0, varxydt;
00230 
00231         Qrot = Rotmatrix(X(2,0));
00232 
00233         Qsubrot = Qrot * Qsub * trans(Qrot);
00234 
00235         //Generate Q
00236         Matrix<float, 4, 4> Q;//(Qsubrot);
00237         Q = Qsubrot(0,0),   Qsubrot(0,1),   0,                  0,
00238             Qsubrot(1,0),   Qsubrot(1,1),   0,                  0,
00239             0,              0,              varang + varangdt,  0,
00240             0,              0,              0,                  0;
00241 
00242         P = F * P * trans(F) + Q;
00243 
00244         //printf("x: %f, y: %f, t: %f\r\n", X(0,0), X(1,0), X(2,0));
00245         //Update Printing
00246         float statecpy[] = {X(0,0), X(1,0), X(2,0)};
00247         Printing::updateval(0, statecpy, 3);
00248         
00249         Printing::updateval(8, X(3,0));
00250 
00251         float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
00252         Printing::updateval(1, Pcpy, 4);
00253 
00254         statelock.unlock();
00255     }
00256 }
00257 
00258 
00259 void predict_event_setter()
00260 {
00261     if(predict_thread_ptr)
00262         predict_thread_ptr->signal_set(0x1);
00263     else
00264         OLED4 = 1;
00265 }
00266 
00267 void start_predict_ticker(Thread* predict_thread_ptr_in)
00268 {
00269     predict_thread_ptr = predict_thread_ptr_in;
00270     predictticker.attach(predict_event_setter, KALMAN_PREDICT_PERIOD);
00271 }
00272 
00273 void runupdate(measurement_t type, float value, float variance)
00274 {
00275     sensorseenflags |= 1<<type;
00276     RawReadings[type] = value;
00277 
00278     if (Kalman_inited) {
00279         measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
00280         if (measured) {
00281             measured->mtype = type;
00282             measured->value = RawReadings[type];
00283             measured->variance = variance;
00284 
00285             osStatus putret = measureMQ.put(measured);
00286             //if (putret)
00287             //OLED4 = 1;
00288             //    printf("putting in MQ error code %#x\r\n", putret);
00289         } else {
00290             //OLED4 = 1;
00291             //printf("MQalloc returned NULL ptr\r\n");
00292         }
00293 
00294     }
00295 
00296 
00297 }
00298 
00299 void Kalman::updateloop(void const*)
00300 {
00301 
00302     //sonar Y chanels
00303     OLED4 = !Printing::registerID(2, 1);
00304     OLED4 = !Printing::registerID(3, 1);
00305     OLED4 = !Printing::registerID(4, 1);
00306 
00307     //IR Y chanels
00308     OLED4 = !Printing::registerID(5, 1);
00309     OLED4 = !Printing::registerID(6, 1);
00310     OLED4 = !Printing::registerID(7, 1);
00311 
00312     bool aborton2stddev = false;
00313 
00314     Matrix<float, 1, 4> H;
00315 
00316     float Y,S;
00317     const Matrix<float, 4, 4> I4( identity< Matrix<float, 4, 4> >() );
00318 
00319 
00320     while (1) {
00321         OLED2 = !OLED2;
00322 
00323         osEvent evt = measureMQ.get();
00324 
00325         if (evt.status == osEventMail) {
00326 
00327             measurmentdata &measured = *(measurmentdata*)evt.value.p;
00328             measurement_t type = measured.mtype; //Note, may support more measurment types than sonar in the future!
00329             float value = measured.value;
00330             float variance = measured.variance;
00331 
00332             // don't forget to free the memory
00333             measureMQ.free(&measured);
00334 
00335             if (type <= maxmeasure) {
00336 
00337                 if (type <= SONAR2) {
00338 
00339                     float dist = value;
00340                     int sonarid = type;
00341                     aborton2stddev = true;
00342 
00343                     statelock.lock();
00344 
00345                     float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0));
00346                     float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0));
00347 
00348                     float rbx = X(0,0) + fp_ct - beaconpos[sonarid].x;
00349                     float rby = X(1,0) + fp_st - beaconpos[sonarid].y;
00350 
00351                     float expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
00352                     Y = dist - expecdist;
00353 
00354                     //send to ui
00355                     Printing::updateval(sonarid+2, Y);
00356 
00357                     float r_expecdist = 1.0f/expecdist;
00358 
00359                     float dhdx = rbx * r_expecdist;
00360                     float dhdy = rby * r_expecdist;
00361                     float dhdt = fp_ct*dhdy - fp_st*dhdx;
00362 
00363                     H = dhdx, dhdy, dhdt, 0;
00364 
00365                 } else if (type <= IR2) {
00366 
00367                     aborton2stddev = true;
00368                     int IRidx = type-3;
00369 
00370                     statelock.lock();
00371 
00372                     float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0));
00373                     float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0));
00374 
00375                     float brx = beaconpos[IRidx].x - (X(0,0) + fp_ct);
00376                     float bry = beaconpos[IRidx].y - (X(1,0) + fp_st);
00377 
00378                     float expecang = atan2(bry, brx) - X(2,0) + X(3,0); //constrainAngle can be called late
00379                     Y = constrainAngle(value - expecang);
00380 
00381                     //send to ui
00382                     Printing::updateval(IRidx + 5, Y);
00383 
00384                     float r_dstsq = 1.0f/(brx*brx + bry*bry);
00385                     float dhdx = -bry*r_dstsq;
00386                     float dhdy = brx*r_dstsq;
00387                     float dhdt = fp_ct*dhdy - fp_st*dhdx - 1.0f;
00388                     float dhdp = 1;
00389                     H = dhdx, dhdy, dhdt, dhdp;
00390                 }
00391 
00392                 Matrix<float, 4, 1> PHt (P * trans(H));
00393                 S = (H * PHt)(0,0) + variance*4; //TODO: Temp Hack!
00394 
00395                 OLED3 = 0;
00396                 if (aborton2stddev && Y*Y > 4 * S) {
00397                     OLED3 = 1;
00398                     statelock.unlock();
00399                     continue;
00400                 }
00401 
00402                 Matrix<float, 4, 1> K (PHt * (1/S));
00403 
00404                 //Updating state
00405                 X += K * Y;
00406                 X(2,0) = constrainAngle(X(2,0));
00407                 X(3,0) = constrainAngle(X(3,0));
00408 
00409                 P = (I4 - K * H) * P;
00410 
00411                 statelock.unlock();
00412 
00413             }
00414 
00415         } else {
00416             OLED4 = 1;
00417             //printf("ERROR: in updateloop, code %#x", evt);
00418         }
00419 
00420     }
00421 
00422 }
00423 
00424 
00425 } //Kalman Namespace