2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/Kalman/Kalman.cpp
- Revision:
- 26:7cb3a21d9a2e
- Parent:
- 22:6e3218cf75f8
- Child:
- 27:664e81033846
diff -r b16f1045108f -r 7cb3a21d9a2e Processes/Kalman/Kalman.cpp --- a/Processes/Kalman/Kalman.cpp Wed Apr 10 02:01:51 2013 +0000 +++ b/Processes/Kalman/Kalman.cpp Wed Apr 10 03:46:23 2013 +0000 @@ -21,6 +21,7 @@ DigitalOut OLED4(LED4); DigitalOut OLED1(LED1); +DigitalOut OLED2(LED2); //State variables Matrix<float, 3, 1> X; @@ -95,7 +96,7 @@ printf("Offsets IR: %0.4f\r\n",IRphaseOffset*180/PI); statelock.lock(); - X(0,0) = x_coor; + X(0,0) = x_coor-TURRET_FWD_PLACEMENT; //assume facing east X(1,0) = y_coor; X(2,0) = 0; @@ -248,29 +249,26 @@ } -/* + void Kalman::updateloop(void const*) { //sonar Y chanels - ui.regid(2, 1); - ui.regid(3, 1); - ui.regid(4, 1); + OLED4 = !Printing::registerID(2, 1); + OLED4 = !Printing::registerID(3, 1); + OLED4 = !Printing::registerID(4, 1); //IR Y chanels - ui.regid(5, 1); - ui.regid(6, 1); - ui.regid(7, 1); + OLED4 = !Printing::registerID(5, 1); + OLED4 = !Printing::registerID(6, 1); + OLED4 = !Printing::registerID(7, 1); - measurement_t type; - float value,variance,rbx,rby,expecdist,Y; - float dhdx,dhdy; bool aborton2stddev = false; Matrix<float, 1, 3> H; - float S; - Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() ); + float Y,S; + const Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() ); while (1) { @@ -281,66 +279,67 @@ if (evt.status == osEventMail) { measurmentdata &measured = *(measurmentdata*)evt.value.p; - type = measured.mtype; //Note, may support more measurment types than sonar in the future! - value = measured.value; - variance = measured.variance; + measurement_t type = measured.mtype; //Note, may support more measurment types than sonar in the future! + float value = measured.value; + float variance = measured.variance; // don't forget to free the memory measureMQ.free(&measured); if (type <= maxmeasure) { - if (type <= SONAR3) { + if (type <= SONAR2) { - InitLock.lock(); - float dist = value / 1000.0f - Sonar_Offset; //converting to m from mm,subtract the offset - InitLock.unlock(); - + float dist = value; int sonarid = type; aborton2stddev = true; statelock.lock(); - //update the current sonar readings - SonarMeasures[sonarid] = dist; - - rbx = X(0) - beaconpos[sonarid].x/1000.0f; - rby = X(1) - beaconpos[sonarid].y/1000.0f; - - expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby); + + float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0)); + float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0)); + + float rbx = X(0,0) + fp_ct - beaconpos[sonarid].x; + float rby = X(1,0) + fp_st - beaconpos[sonarid].y; + + float expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby); Y = dist - expecdist; //send to ui - ui.updateval(sonarid+2, Y); + Printing::updateval(sonarid+2, Y); - dhdx = rbx / expecdist; - dhdy = rby / expecdist; + float r_expecdist = 1.0f/expecdist; - H = dhdx, dhdy, 0; + float dhdx = rbx * r_expecdist; + float dhdy = rby * r_expecdist; + float dhdt = (fp_ct*rby - fp_st*rbx) * r_expecdist; - } else if (type <= IR3) { + H = dhdx, dhdy, dhdt; - aborton2stddev = false; + } else if (type <= IR2) { + + aborton2stddev = true; int IRidx = type-3; - // subtract the IR offset - InitLock.lock(); - value -= IR_Offset; - InitLock.unlock(); + statelock.lock(); + + float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0)); + float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0)); - statelock.lock(); - IRMeasures[IRidx] = value; + float brx = beaconpos[IRidx].x - (X(0,0) + fp_ct); + float bry = beaconpos[IRidx].y - (X(1,0) + fp_st); - rbx = X(0) - beaconpos[IRidx].x/1000.0f; - rby = X(1) - beaconpos[IRidx].y/1000.0f; - - float expecang = atan2(-rby, -rbx) - X(2); - Y = rectifyAng(value - expecang); + float expecang = atan2(bry, brx) - X(2,0); //constrainAngle can be called late + Y = constrainAngle(value - expecang); //send to ui - ui.updateval(IRidx + 5, Y); + Printing::updateval(IRidx + 5, Y); - float dstsq = rbx*rbx + rby*rby; - H = -rby/dstsq, rbx/dstsq, -1; + float r_dstsq = 1.0f/(brx*brx + bry*bry); + float dhdx = -bry*r_dstsq; + float dhdy = brx*r_dstsq; + float dhdt = fp_ct*dhdy - fp_st*dhdx - 1.0f; + H = dhdx, dhdy, dhdt; } Matrix<float, 3, 1> PH (P * trans(H)); @@ -354,8 +353,8 @@ Matrix<float, 3, 1> K (PH * (1/S)); //Updating state - X += col(K, 0) * Y; - X(2) = rectifyAng(X(2)); + X += K * Y; + X(2,0) = constrainAngle(X(2,0)); P = (I3 - K * H) * P; @@ -372,6 +371,5 @@ } -*/ } //Kalman Namespace