2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/Kalman/Kalman.cpp
- Revision:
- 48:254b124cef02
- Parent:
- 38:c9058a401410
- Child:
- 49:665bdca0f2cd
diff -r fc471218af95 -r 254b124cef02 Processes/Kalman/Kalman.cpp --- a/Processes/Kalman/Kalman.cpp Fri Apr 12 02:05:51 2013 +0000 +++ b/Processes/Kalman/Kalman.cpp Fri Apr 12 17:03:53 2013 +0000 @@ -20,17 +20,17 @@ Ticker predictticker; DigitalOut OLED4(LED4); +DigitalOut OLED3(LED3); DigitalOut OLED1(LED1); DigitalOut OLED2(LED2); //State variables -Matrix<float, 3, 1> X; -Matrix<float, 3, 3> P; +Matrix<float, 4, 1> X; +Matrix<float, 4, 4> P; Mutex statelock; float RawReadings[maxmeasure+1]; int sensorseenflags = 0; -float IRphaseOffset; bool Kalman_inited = 0; @@ -49,12 +49,12 @@ 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) ); - + //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; @@ -62,18 +62,18 @@ float r1 = RawReadings[SONAR2]; float r2 = RawReadings[SONAR1]; float r3 = RawReadings[SONAR0]; - + 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; - + //coordinate system hack (for now) x_coor = beaconpos[2].x - x_coor; y_coor = beaconpos[2].y - y_coor; - + printf("solved pos from sonar: %f, %f \r\n", x_coor, y_coor); - + //IR float IRMeasuresloc[3]; IRMeasuresloc[0] = RawReadings[IR0]; @@ -84,52 +84,55 @@ float IR_Offsets[3] = {0}; float frombrefoffset = 0; int refbeacon = 0; - - for (int i = 0; i < 3; i++){ - if (sensorseenflags & 1<<(3+i)){ + + 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)){ + 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]); } } - IRphaseOffset = constrainAngle(IR_Offsets[refbeacon] + frombrefoffset/cnt); + X(3,0) = constrainAngle(IR_Offsets[refbeacon] + frombrefoffset/cnt); //debug - printf("Offsets IR: %0.4f\r\n",IRphaseOffset*180/PI); + printf("Offsets IR: %0.4f\r\n",X(3,0)*180/PI); statelock.lock(); - X(0,0) = x_coor-TURRET_FWD_PLACEMENT; //assume facing east + X(0,0) = x_coor-TURRET_FWD_PLACEMENT; X(1,0) = y_coor; - X(2,0) = 0; - - P = 0.02*0.02, 0, 0, - 0, 0.02*0.02, 0, - 0, 0, 0.04; + 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.4*0.4, -0.4*0.4, + 0, 0, -0.4*0.4, 0.4*0.4; + statelock.unlock(); - + Kalman_inited = 1; } -State getState(){ +State getState() +{ statelock.lock(); State state = {X(0,0), X(1,0), X(2,0)}; statelock.unlock(); @@ -182,13 +185,14 @@ 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 - float avgX2 = (X(2,0) + 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; + 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; @@ -206,10 +210,11 @@ 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; + 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; @@ -226,14 +231,16 @@ } -void predict_event_setter(){ +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){ +void start_predict_ticker(Thread* predict_thread_ptr_in) +{ predict_thread_ptr = predict_thread_ptr_in; predictticker.attach(predict_event_setter, KALMAN_PREDICT_PERIOD); } @@ -242,16 +249,7 @@ { sensorseenflags |= 1<<type; - if (!Kalman_inited) { - RawReadings[type] = value; - } else { - - if (type >= IR0 && type <= IR2) - RawReadings[type] = value - IRphaseOffset; - else - RawReadings[type] = value; - - + if (Kalman_inited) { measurmentdata* measured = (measurmentdata*)measureMQ.alloc(); if (measured) { measured->mtype = type; @@ -260,15 +258,15 @@ osStatus putret = measureMQ.put(measured); //if (putret) - //OLED4 = 1; + //OLED4 = 1; // printf("putting in MQ error code %#x\r\n", putret); } else { //OLED4 = 1; //printf("MQalloc returned NULL ptr\r\n"); } - + } - + } @@ -287,10 +285,10 @@ bool aborton2stddev = false; - Matrix<float, 1, 3> H; + Matrix<float, 1, 4> H; float Y,S; - const Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() ); + const Matrix<float, 4, 4> I4( identity< Matrix<float, 4, 4> >() ); while (1) { @@ -317,13 +315,13 @@ 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; @@ -336,7 +334,7 @@ float dhdy = rby * r_expecdist; float dhdt = fp_ct*dhdy - fp_st*dhdx; - H = dhdx, dhdy, dhdt; + H = dhdx, dhdy, dhdt, 0; } else if (type <= IR2) { @@ -344,14 +342,14 @@ 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); //constrainAngle can be called late + float expecang = atan2(bry, brx) - X(2,0) + X(3,0); //constrainAngle can be called late Y = constrainAngle(value - expecang); //send to ui @@ -361,24 +359,28 @@ float dhdx = -bry*r_dstsq; float dhdy = brx*r_dstsq; float dhdt = fp_ct*dhdy - fp_st*dhdx - 1.0f; - H = dhdx, dhdy, dhdt; + float dhdp = 1; + H = dhdx, dhdy, dhdt, dhdp; } - Matrix<float, 3, 1> PH (P * trans(H)); - S = (H * PH)(0,0) + variance*10; //TODO: temp hack + Matrix<float, 4, 1> PHt (P * trans(H)); + S = (H * PHt)(0,0) + variance; + OLED3 = 0; if (aborton2stddev && Y*Y > 4 * S) { + OLED3 = 1; statelock.unlock(); continue; } - Matrix<float, 3, 1> K (PH * (1/S)); + 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 = (I3 - K * H) * P; + P = (I4 - K * H) * P; statelock.unlock();