Eurobot_shared pubulished from Eurobot Primary
Kalman/Kalman.cpp
- Committer:
- narshu
- Date:
- 2012-08-07
- Revision:
- 0:434fd09723be
File content as of revision 0:434fd09723be:
//*************************************************************************************** //Kalman Filter implementation //*************************************************************************************** #include "Kalman.h" #include "rtos.h" #include "RFSRF05.h" #include "math.h" #include "globals.h" #include "motors.h" #include "system.h" #include "geometryfuncs.h" #include <tvmet/Matrix.h> #include <tvmet/Vector.h> using namespace tvmet; Kalman::Kalman(Motors &motorsin, UI &uiin, PinName Sonar_Trig, PinName Sonar_Echo0, PinName Sonar_Echo1, PinName Sonar_Echo2, PinName Sonar_Echo3, PinName Sonar_Echo4, PinName Sonar_Echo5, PinName Sonar_SDI, PinName Sonar_SDO, PinName Sonar_SCK, PinName Sonar_NCS, PinName Sonar_NIRQ) : ir(*this), sonararray(Sonar_Trig, Sonar_Echo0, Sonar_Echo1, Sonar_Echo2, Sonar_Echo3, Sonar_Echo4, Sonar_Echo5, Sonar_SDI, Sonar_SDO, Sonar_SCK, Sonar_NCS, Sonar_NIRQ), motors(motorsin), ui(uiin), predictthread(predictloopwrapper, this, osPriorityNormal, 512), predictticker( SIGTICKARGS(predictthread, 0x1) ), // sonarthread(sonarloopwrapper, this, osPriorityNormal, 256), // sonarticker( SIGTICKARGS(sonarthread, 0x1) ), updatethread(updateloopwrapper, this, osPriorityNormal, 512) { //Initilising offsets InitLock.lock(); IR_Offset = 0; Sonar_Offset = 0; InitLock.unlock(); //Initilising matrices // X = x, y, theta; if (Colour) X = 0.5, 0, 0; else X = 2.5, 0, PI; P = 1, 0, 0, 0, 1, 0, 0, 0, 0.04; //measurment variance R is provided by each sensor when calling runupdate //attach callback sonararray.callbackobj = (DummyCT*)this; sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance, float variance)) &Kalman::runupdate; predictticker.start(20); // sonarticker.start(50); } //Note: this init function assumes that the robot faces east, theta=0, in the +x direction void Kalman::KalmanInit() { motors.stop(); float SonarMeasuresx1000[3]; float IRMeasuresloc[3]; int beacon_cnt = 0; // doesn't work since they break the ISR /* #ifdef ROBOT_PRIMARY LPC_UART3->FCR = LPC_UART3->FCR | 0x06; // Flush the serial FIFO buffer / OR with FCR #else LPC_UART1->FCR = LPC_UART1->FCR | 0x06; // Flush the serial FIFO buffer / OR with FCR #endif */ // zeros the measurements for (int i = 0; i < 3; i++) { SonarMeasures[i] = 0; IRMeasures[i] = 0; } InitLock.lock(); //zeros offsets IR_Offset = 0; Sonar_Offset = 0; InitLock.unlock(); // attaches ir interrup ir.attachisr(); //wating untill the IR has reved up and picked up some valid data //Thread::wait(1000); wait(2); //temporaraly disable IR updates ir.detachisr(); //lock the state throughout the computation, as we will override the state at the end InitLock.lock(); statelock.lock(); SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f; SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f; SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f; IRMeasuresloc[0] = IRMeasures[0]; IRMeasuresloc[1] = IRMeasures[1]; IRMeasuresloc[2] = IRMeasures[2]; //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI); float d = beaconpos[2].y - beaconpos[1].y; float i = beaconpos[0].y - beaconpos[1].y; float j = beaconpos[0].x - beaconpos[1].x; float origin_x = beaconpos[1].x; float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d); float x_coor = origin_x + (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j; //debug for trilateration printf("Cal at x: %0.4f, y: %0.4f \r\n",x_coor,y_coor ); float Dist_Exp[3]; for (int i = 0; i < 3; i++) { //Compute sonar offset Dist_Exp[i] = hypot(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor); Sonar_Offset += (SonarMeasuresx1000[i]-Dist_Exp[i])/3000.0f; //Compute IR offset float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor); if (!Colour) angle_est -= PI; //printf("Angle %d : %f \n\r",i,angle_est*180/PI ); // take average offset angle from valid readings if (IRMeasuresloc[i] != 0) { beacon_cnt ++; // changed to current angle - estimated angle float angle_temp = IRMeasuresloc[i] - angle_est; angle_temp -= (floor(angle_temp/(2*PI)))*2*PI; IR_Offset += angle_temp; } } IR_Offset /= float(beacon_cnt); //debug printf("Offsets IR: %0.4f, Sonar: %0.4f \r\n",IR_Offset*180/PI,Sonar_Offset*1000 ); //statelock already locked X(0) = x_coor/1000.0f; X(1) = y_coor/1000.0f; if (Colour) X(2) = 0; else X(2) = PI; // unlocks mutexes InitLock.unlock(); statelock.unlock(); //reattach the IR processing ir.attachisr(); } void Kalman::predictloop() { OLED4 = !ui.regid(0, 3); OLED4 = !ui.regid(1, 4); float lastleft = 0; float lastright = 0; while (1) { Thread::signal_wait(0x1); OLED1 = !OLED1; int leftenc = motors.getEncoder1(); int rightenc = motors.getEncoder2(); float dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f; float dright = motors.encoderToDistance(rightenc-lastright)/1000.0f; lastleft = leftenc; lastright = rightenc; //The below calculation are in body frame (where +x is forward) float dxp, dyp,d,r; float thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f); if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error d = (dright + dleft)/2.0f; dxp = d*cos(thetap/2.0f); dyp = d*sin(thetap/2.0f); } else { //calculate circle arc //float r = (right + left) / (4.0f * PI * thetap); r = (dright + dleft) / (2.0f*thetap); dxp = abs(r)*sin(thetap); dyp = r - r*cos(thetap); } statelock.lock(); float tempX2 = X(2); //rotating to cartesian frame and updating state X(0) += dxp * cos(X(2)) - dyp * sin(X(2)); X(1) += dxp * sin(X(2)) + dyp * cos(X(2)); X(2) = rectifyAng(X(2) + thetap); //Linearising F around X float avgX2 = (X(2) + tempX2)/2.0f; Matrix<float, 3, 3> F; F = 1, 0, (dxp * -sin(avgX2) - dyp * cos(avgX2)), 0, 1, (dxp * cos(avgX2) - dyp * sin(avgX2)), 0, 0, 1; //Generating forward and rotational variance float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f; float varang = varperang * abs(thetap); float varxydt = xyvarpertime * PREDICTPERIOD/1000.0f; float varangdt = angvarpertime * PREDICTPERIOD/1000.0f; //Rotating into cartesian frame Matrix<float, 2, 2> Qsub,Qsubrot,Qrot; Qsub = varfwd + varxydt, 0, 0, varxydt; Qrot = Rotmatrix(X(2)); Qsubrot = Qrot * Qsub * trans(Qrot); //Generate Q Matrix<float, 3, 3> Q;//(Qsubrot); Q = Qsubrot(0,0), Qsubrot(0,1), 0, Qsubrot(1,0), Qsubrot(1,1), 0, 0, 0, varang + varangdt; P = F * P * trans(F) + Q; //Update UI float statecpy[] = {X(0), X(1), X(2)}; ui.updateval(0, statecpy, 3); float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)}; ui.updateval(1, Pcpy, 4); statelock.unlock(); } } //void Kalman::sonarloop() { // while (1) { // Thread::signal_wait(0x1); // sonararray.startRange(); // } //} void Kalman::runupdate(measurement_t type, float value, float variance) { //printf("beacon %d dist %f\r\n", sonarid, dist); //led2 = !led2; measurmentdata* measured = (measurmentdata*)measureMQ.alloc(); if (measured) { measured->mtype = type; measured->value = value; measured->variance = variance; osStatus putret = measureMQ.put(measured); if (putret) OLED4 = 1; // printf("putting in MQ error code %#x\r\n", putret); } else { OLED4 = 1; //printf("MQalloc returned NULL ptr\r\n"); } } void Kalman::updateloop() { //sonar Y chanels ui.regid(2, 1); ui.regid(3, 1); ui.regid(4, 1); //IR Y chanels ui.regid(5, 1); ui.regid(6, 1); ui.regid(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> >() ); while (1) { OLED2 = !OLED2; osEvent evt = measureMQ.get(); 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; // don't forget to free the memory measureMQ.free(&measured); if (type <= maxmeasure) { if (type <= SONAR3) { InitLock.lock(); float dist = value / 1000.0f - Sonar_Offset; //converting to m from mm,subtract the offset InitLock.unlock(); 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); Y = dist - expecdist; //send to ui ui.updateval(sonarid+2, Y); dhdx = rbx / expecdist; dhdy = rby / expecdist; H = dhdx, dhdy, 0; } else if (type <= IR3) { aborton2stddev = false; int IRidx = type-3; // subtract the IR offset InitLock.lock(); value -= IR_Offset; InitLock.unlock(); statelock.lock(); IRMeasures[IRidx] = value; 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); //send to ui ui.updateval(IRidx + 5, Y); float dstsq = rbx*rbx + rby*rby; H = -rby/dstsq, rbx/dstsq, -1; } Matrix<float, 3, 1> PH (P * trans(H)); S = (H * PH)(0,0) + variance; if (aborton2stddev && Y*Y > 4 * S) { statelock.unlock(); continue; } Matrix<float, 3, 1> K (PH * (1/S)); //Updating state X += col(K, 0) * Y; X(2) = rectifyAng(X(2)); P = (I3 - K * H) * P; statelock.unlock(); } } else { OLED4 = 1; //printf("ERROR: in updateloop, code %#x", evt); } } } // reset kalman states void Kalman::KalmanReset() { float SonarMeasuresx1000[3]; statelock.lock(); SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f; SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f; SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f; //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI); float d = beaconpos[2].y - beaconpos[1].y; float i = beaconpos[0].y - beaconpos[1].y; float j = beaconpos[0].x - beaconpos[1].x; float origin_x = beaconpos[1].x; float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d); float x_coor = origin_x +(SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j; //statelock already locked X(0) = x_coor/1000.0f; X(1) = y_coor/1000.0f; /* if (Colour){ X(0) = 0.2; X(1) = 0.2; //X(2) = 0; } else { X(0) = 2.8; X(1) = 0.2; //X(2) = PI; } */ P = 0.05, 0, 0, 0, 0.05, 0, 0, 0, 0.04; // unlocks mutexes statelock.unlock(); }