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