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