![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ICRS Eurobot 2013
Dependencies: mbed mbed-rtos Servo QEI
Diff: Processes/Kalman/Kalman.cpp
- Revision:
- 19:4b993a9a156e
- Parent:
- 17:6263e90bf3ba
- Child:
- 20:70d651156779
--- a/Processes/Kalman/Kalman.cpp Sun Apr 07 17:51:59 2013 +0000 +++ b/Processes/Kalman/Kalman.cpp Sun Apr 07 19:26:07 2013 +0000 @@ -16,12 +16,12 @@ { //State variables -Vector<float, 3> X; +Matrix<float, 3, 1> X; Matrix<float, 3, 3> P; Mutex statelock; float RawReadings[maxmeasure+1]; -float SensorOffsets[maxmeasure+1] = {0}; +float IRpahseOffset; bool Kalman_init = 0; @@ -29,7 +29,7 @@ measurement_t mtype; float value; float variance; -} +}; Mail <measurmentdata, 16> measureMQ; @@ -38,21 +38,15 @@ //Note: this init function assumes that the robot faces east, theta=0, in the +x direction void KalmanInit() { - - //Solving for sonar bias is done by entering the following into wolfram alpha - //(a-f)^2 = x^2 + y^2; (b-f)^2 = (x-3)^2 + y^2; (c-f)^2 = (x-1.5)^2+(y-2)^2: solve for x,y,f - //where a, b, c are the measured distances, and f is the bias - - SensorOffsets[SONAR0] = sonartimebias; - SensorOffsets[SONAR1] = sonartimebias; - SensorOffsets[SONAR2] = sonartimebias; - //solve for our position (assume perfect bias) const float d = beaconpos[0].y - beaconpos[1].y; const float i = beaconpos[0].y - beaconpos[2].y; const float j = beaconpos[0].x - beaconpos[2].x; + float r1 = RawReadings[SONAR0]; + float r2 = RawReadings[SONAR1]; + float r3 = RawReadings[SONAR2]; - float y_coor = (r1*r1-r2*r2+d*d)/2d; + 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; //IR @@ -71,32 +65,24 @@ 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_Offset[i] = constrainAngle(IRMeasuresloc[i] - angle_est); + IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est); - fromb0offset += IR_Offsets[i] - IR_Offset[0]; + fromb0offset += constrainAngle(IR_Offsets[i] - IR_Offsets[0]); } - + + IRpahseOffset = constrainAngle(IR_Offsets[0] + fromb0offset/3); //debug - printf("Offsets IR: %0.4f, Sonar: %0.4f \r\n",IR_Offset*180/PI,Sonar_Offset*1000 ); - + printf("Offsets IR: %0.4f\r\n",IRpahseOffset*180/PI); statelock.lock(); - X(0) = x_coor/1000.0f; - X(1) = y_coor/1000.0f; - - if (Colour) - X(2) = 0; - else - X(2) = PI; + X(0,0) = x_coor; + X(1,0) = y_coor; + X(2,0) = 0; statelock.unlock(); - - - //reattach the IR processing - ir.attachisr(); } - +/* void Kalman::predictloop(void* dummy) { @@ -355,19 +341,6 @@ 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; @@ -377,5 +350,7 @@ } +*/ + } //Kalman Namespace