2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/Kalman/Kalman.cpp
- Revision:
- 16:52250d8d8fce
- Child:
- 17:6263e90bf3ba
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Processes/Kalman/Kalman.cpp Sun Apr 07 16:50:36 2013 +0000 @@ -0,0 +1,383 @@ +//*************************************************************************************** +//Kalman Filter implementation +//*************************************************************************************** +#include "Kalman.h" +#include "rtos.h" +#include "math.h" +#include "supportfuncs.h" +//#include "globals.h" + +#include <tvmet/Matrix.h> +using namespace tvmet; + + + +namespace Kalman +{ + +//State variables +Vector<float, 3> X; +Matrix<float, 3, 3> P; +Mutex statelock; + +float RawReadings[maxmeasure+1]; +float SensorOffsets[maxmeasure+1] = {0}; + +bool Kalman_init = 0; + +struct measurmentdata { + measurement_t mtype; + float value; + float variance; +} + +Mail <measurmentdata, 16> measureMQ; + + + +//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) + float x_coor = + + + //IR + + float IRMeasuresloc[3]; + + IRMeasuresloc[0] = RawReadings[IR0]; + IRMeasuresloc[1] = RawReadings[IR1]; + IRMeasuresloc[2] = RawReadings[IR2]; + //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI); + + IR_Offset = 0; + for (int i = 0; i < 3; i++) { + + //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) { + // 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.lock(); + X(0) = x_coor/1000.0f; + X(1) = y_coor/1000.0f; + + if (Colour) + X(2) = 0; + else + X(2) = PI; + statelock.unlock(); + + + //reattach the IR processing + ir.attachisr(); +} + + +void Kalman::predictloop(void* dummy) +{ + + 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 = encoders.getEncoder1(); + int rightenc = encoders.getEncoder2(); + + float dleft = encoders.encoderToDistance(leftenc-lastleft)/1000.0f; + float dright = encoders.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::runupdate(measurement_t type, float value, float variance) +{ + if (!Kalman_init) + RawReadings[type] = value; + else { + + RawReadings[type] = value - SensorOffsets[type]; + + 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(void* dummy) +{ + + //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(); + +} + +} //Kalman Namespace +