2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Processes/Kalman/Kalman.cpp
- Committer:
- madcowswe
- Date:
- 2013-04-15
- Revision:
- 79:0d3140048526
- Parent:
- 72:7996aa8286ae
- Child:
- 85:b0858346d838
File content as of revision 79:0d3140048526:
//*************************************************************************************** //Kalman Filter implementation //*************************************************************************************** #include "Kalman.h" #include "rtos.h" #include "math.h" #include "supportfuncs.h" #include "Encoder.h" #include "globals.h" #include "Printing.h" #include "tvmet/Matrix.h" using namespace tvmet; namespace Kalman { Ticker predictticker; DigitalOut OLED4(LED4); DigitalOut OLED3(LED3); DigitalOut OLED1(LED1); DigitalOut OLED2(LED2); //State variables Matrix<float, 4, 1> X; Matrix<float, 4, 4> P; Mutex statelock; float RawReadings[maxmeasure+1]; volatile int sensorseenflags = 0; bool Kalman_inited = 0; struct measurmentdata { measurement_t mtype; float value; float variance; }; Mail <measurmentdata, 16> measureMQ; Thread* predict_thread_ptr = NULL; //Note: this init function assumes that the robot faces east, theta=0, in the +x direction void KalmanInit() { printf("kalmaninit \r\n"); //WARNING: HARDCODED! TODO: fix it so it works for both sides! printf("waiting for all sonar, and at least 1 IR\r\n"); while( ((sensorseenflags & 0x7)^0x7) || !(sensorseenflags & 0x7<<3) ); #ifdef TEAM_RED //solve for our position (assume perfect bias) const float d = beaconpos[2].y - beaconpos[1].y; const float i = beaconpos[2].y - beaconpos[0].y; const float j = beaconpos[2].x - beaconpos[0].x; float r1 = RawReadings[SONAR2]; float r2 = RawReadings[SONAR1]; float r3 = RawReadings[SONAR0]; #endif #ifdef TEAM_BLUE const float d = beaconpos[1].y - beaconpos[2].y; const float i = beaconpos[0].y - beaconpos[2].y; const float j = beaconpos[0].x - beaconpos[2].x; float r1 = RawReadings[SONAR2]; float r2 = RawReadings[SONAR1]; float r3 = RawReadings[SONAR0]; #endif printf("ranges: 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", r1, r2, r3); float y_coor = (r1*r1-r2*r2+d*d)/(2*d); float x_coor = (r1*r1-r3*r3+i*i+j*j)/(2*j) - (i*y_coor)/j; #ifdef TEAM_RED //coordinate system hack (for now) x_coor = beaconpos[2].x - x_coor; y_coor = beaconpos[2].y - y_coor; #endif #ifdef TEAM_BLUE x_coor = x_coor - beaconpos[2].x; y_coor = y_coor - beaconpos[2].y; #endif printf("solved pos from sonar: %f, %f \r\n", x_coor, y_coor); //IR float IRMeasuresloc[3]; IRMeasuresloc[0] = RawReadings[IR0]; IRMeasuresloc[1] = RawReadings[IR1]; IRMeasuresloc[2] = RawReadings[IR2]; 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); float IR_Offsets[3] = {0}; float frombrefoffset = 0; int refbeacon = 0; for (int i = 0; i < 3; i++) { if (sensorseenflags & 1<<(3+i)) { refbeacon = i; break; } } printf("refbeacon is %d\r\n", refbeacon); int cnt = 0; for (int i = 0; i < 3; i++) { if (sensorseenflags & 1<<(3+i)) { cnt++; //Compute IR offset float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor); //printf("Angle %d : %f \n\r",i,angle_est*180/PI ); IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est); frombrefoffset += constrainAngle(IR_Offsets[i] - IR_Offsets[refbeacon]); } } printf("Used IR info from %d beacons\r\n", cnt); X(3,0) = constrainAngle(IR_Offsets[refbeacon] + frombrefoffset/cnt); //debug printf("Offsets IR: %0.4f\r\n",X(3,0)*180/PI); statelock.lock(); X(0,0) = x_coor-TURRET_FWD_PLACEMENT; X(1,0) = y_coor; X(2,0) = 0; //TODO: assume facing east, need to account for both starting pos P = 0.02*0.02, 0, 0, 0, 0, 0.02*0.02, 0, 0, 0, 0, 0.1*0.1, -0.1*0.1, 0, 0, -0.1*0.1, 0.1*0.1 + 0.05*0.05; statelock.unlock(); Kalman_inited = 1; } State getState() { statelock.lock(); State state = {X(0,0), X(1,0), X(2,0)}; statelock.unlock(); return state; } void predictloop(void const*) { OLED4 = !Printing::registerID(0, 3) || OLED4; OLED4 = !Printing::registerID(1, 4) || OLED4; OLED4 = !Printing::registerID(8, 1) || OLED4; float lastleft = 0; float lastright = 0; while (1) { Thread::signal_wait(0x1); OLED1 = !OLED1; float leftenc = left_encoder.getTicks() * ENCODER_M_PER_TICK; float rightenc = right_encoder.getTicks() * ENCODER_M_PER_TICK; float dleft = leftenc-lastleft; float dright = rightenc-lastright; lastleft = leftenc; lastright = rightenc; //The below calculation are in body frame (where +x is forward) float dxp, dyp,d,r; float thetap = (dright - dleft) / ENCODER_WHEELBASE; if (abs(thetap) < 0.01f) { //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 = r*sin(thetap); dyp = r - r*cos(thetap); } statelock.lock(); float tempX2 = X(2,0); //rotating to cartesian frame and updating state X(0,0) += dxp * cos(X(2,0)) - dyp * sin(X(2,0)); X(1,0) += dxp * sin(X(2,0)) + dyp * cos(X(2,0)); X(2,0) = constrainAngle(X(2,0) + thetap); //X(3,0) += 0; //Linearising F around X Matrix<float, 4, 4> F; F = 1, 0, (dxp * -sin(tempX2) - dyp * cos(tempX2)), 0, 0, 1, (dxp * cos(tempX2) - dyp * sin(tempX2)), 0, 0, 0, 1, 0, 0, 0, 0, 1; //Generating forward and rotational variance float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f; float varang = varperang * abs(thetap); float varxydt = xyvarpertime * KALMAN_PREDICT_PERIOD; float varangdt = angvarpertime * KALMAN_PREDICT_PERIOD; //Rotating into cartesian frame Matrix<float, 2, 2> Qsub,Qsubrot,Qrot; Qsub = varfwd + varxydt, 0, 0, varxydt; Qrot = Rotmatrix(X(2,0)); Qsubrot = Qrot * Qsub * trans(Qrot); //Generate Q Matrix<float, 4, 4> Q;//(Qsubrot); Q = Qsubrot(0,0), Qsubrot(0,1), 0, 0, Qsubrot(1,0), Qsubrot(1,1), 0, 0, 0, 0, varang + varangdt, 0, 0, 0, 0, 0; P = F * P * trans(F) + Q; //printf("x: %f, y: %f, t: %f\r\n", X(0,0), X(1,0), X(2,0)); //Update Printing float statecpy[] = {X(0,0), X(1,0), X(2,0)}; Printing::updateval(0, statecpy, 3); Printing::updateval(8, X(3,0)); float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)}; Printing::updateval(1, Pcpy, 4); statelock.unlock(); } } void predict_event_setter() { if(predict_thread_ptr) predict_thread_ptr->signal_set(0x1); else OLED4 = 1; } void start_predict_ticker(Thread* predict_thread_ptr_in) { predict_thread_ptr = predict_thread_ptr_in; predictticker.attach(predict_event_setter, KALMAN_PREDICT_PERIOD); } void runupdate(measurement_t type, float value, float variance) { sensorseenflags |= 1<<type; RawReadings[type] = value; if (Kalman_inited) { measurmentdata* measured = (measurmentdata*)measureMQ.alloc(); if (measured) { measured->mtype = type; measured->value = RawReadings[type]; 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(void const*) { //sonar Y chanels OLED4 = !Printing::registerID(2, 1); OLED4 = !Printing::registerID(3, 1); OLED4 = !Printing::registerID(4, 1); //IR Y chanels OLED4 = !Printing::registerID(5, 1); OLED4 = !Printing::registerID(6, 1); OLED4 = !Printing::registerID(7, 1); bool aborton2stddev = false; Matrix<float, 1, 4> H; float Y,S; const Matrix<float, 4, 4> I4( identity< Matrix<float, 4, 4> >() ); while (1) { OLED2 = !OLED2; osEvent evt = measureMQ.get(); if (evt.status == osEventMail) { measurmentdata &measured = *(measurmentdata*)evt.value.p; 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 <= SONAR2) { float dist = value; int sonarid = type; aborton2stddev = true; statelock.lock(); 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 Printing::updateval(sonarid+2, Y); float r_expecdist = 1.0f/expecdist; float dhdx = rbx * r_expecdist; float dhdy = rby * r_expecdist; float dhdt = fp_ct*dhdy - fp_st*dhdx; H = dhdx, dhdy, dhdt, 0; } else if (type <= IR2) { aborton2stddev = true; int IRidx = type-3; statelock.lock(); float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0)); float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0)); float brx = beaconpos[IRidx].x - (X(0,0) + fp_ct); float bry = beaconpos[IRidx].y - (X(1,0) + fp_st); float expecang = atan2(bry, brx) - X(2,0) + X(3,0); //constrainAngle can be called late Y = constrainAngle(value - expecang); //send to ui Printing::updateval(IRidx + 5, Y); 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; float dhdp = 1; H = dhdx, dhdy, dhdt, dhdp; } Matrix<float, 4, 1> PHt (P * trans(H)); S = (H * PHt)(0,0) + variance*4; //TODO: Temp Hack! OLED3 = 0; if (aborton2stddev && Y*Y > 4 * S) { OLED3 = 1; statelock.unlock(); continue; } Matrix<float, 4, 1> K (PHt * (1/S)); //Updating state X += K * Y; X(2,0) = constrainAngle(X(2,0)); X(3,0) = constrainAngle(X(3,0)); P = (I4 - K * H) * P; statelock.unlock(); } } else { OLED4 = 1; //printf("ERROR: in updateloop, code %#x", evt); } } } } //Kalman Namespace