Shuto Naruse
/
Eurobot2012_Secondary
Eurobot2012_Secondary
Fork of Eurobot_2012_Secondary by
Diff: Kalman/Kalman.cpp
- Revision:
- 0:fbfafa6bf5f9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Kalman/Kalman.cpp Fri Apr 20 21:32:24 2012 +0000 @@ -0,0 +1,262 @@ +//*************************************************************************************** +//Kalman Filter implementation +//*************************************************************************************** +#include "Kalman.h" +#include "rtos.h" +#include "RFSRF05.h" +//#include "MatrixMath.h" +//#include "Matrix.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; +DigitalOut led1(LED1); +DigitalOut led2(LED2); +DigitalOut led3(LED3); +DigitalOut led4(LED4); + + +Kalman::Kalman(Motors &motorsin) : + sonararray(p10,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9), + motors(motorsin), + predictthread(predictloopwrapper, this, osPriorityNormal, 512), + predictticker( SIGTICKARGS(predictthread, 0x1) ), +// sonarthread(sonarloopwrapper, this, osPriorityNormal, 256), +// sonarticker( SIGTICKARGS(sonarthread, 0x1) ), + updatethread(updateloopwrapper, this, osPriorityNormal, 2048) { + + //Initilising matrices + + // X = x, y, theta; + X = 0.5, 0, 0; + + P = 1, 0, 0, + 0, 1, 0, + 0, 0, 0.04; + + //Q = 0.002, 0, 0, //temporary matrix, Use dt! + // 0, 0.002, 0, + // 0, 0, 0.002; + + //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); + + +} + + +void Kalman::predictloop() { + + float lastleft = 0; + float lastright = 0; + + while (1) { + Thread::signal_wait(0x1); + led1 = !led1; + + 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(); + + //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 + Matrix<float, 3, 3> F; + F = 1, 0, (dxp * -sin(X(2)) - dyp * cos(X(2))), + 0, 1, (dxp * cos(X(2)) - dyp * sin(X(2))), + 0, 0, 1; + + //Generating forward and rotational variance + float varfwd = fwdvarperunit * (dright + dleft) / 2.0f; + float varang = varperang * thetap; + float varxydt = xyvarpertime * PREDICTPERIOD; + float varangdt = angvarpertime * PREDICTPERIOD; + + //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; + + statelock.unlock(); + //Thread::wait(PREDICTPERIOD); + + //cout << "predict" << X << endl; + //cout << P << endl; + } +} + +//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) + led4 = 1; + // printf("putting in MQ error code %#x\r\n", putret); + } else { + led4 = 1; + //printf("MQalloc returned NULL ptr\r\n"); + } + +} + +void Kalman::updateloop() { + 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) { + led2 = !led2; + + 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) { + + float dist = value / 1000.0f; //converting to m from mm + int sonarid = type; + aborton2stddev = false; + + statelock.lock(); + SonarMeasures[sonarid] = dist; //update the current sonar readings + + 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; + + dhdx = rbx / expecdist; + dhdy = rby / expecdist; + + H = dhdx, dhdy, 0; + + } else if (type <= IR3) { + + aborton2stddev = false; + int IRidx = type-3; + + statelock.lock(); + IRMeasures[IRidx] = value; + + rbx = X(0) - beaconpos[IRidx].x/1000.0f; + rby = X(1) - beaconpos[IRidx].y/1000.0f; + + float expecang = atan2(-rbx, -rby) - X(2); + //printf("expecang: %0.4f, value: %0.4f \n\r", expecang*180/PI,value*180/PI); + Y = rectifyAng(value + expecang); + + 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 { + led4 = 1; + //printf("ERROR: in updateloop, code %#x", evt); + } + + } + +} \ No newline at end of file