Sample of program breaking when a certain set of source files are in a folder, but is fine when it is in the root. In this case, it is tested with RF12B.cpp, RF12B.h and rfdefs
Diff: Kalman/Kalman.cpp
- Revision:
- 0:349dc9b0984f
diff -r 000000000000 -r 349dc9b0984f Kalman/Kalman.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Kalman/Kalman.cpp Sun Mar 25 13:39:11 2012 +0000 @@ -0,0 +1,185 @@ +//*************************************************************************************** +//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" + + +Kalman::Kalman() : //Motors &motorsin) : + //X(3,1), + //P(3,3), + //Q(3,3), + //sonararray(p13,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9){ + rflol(p5, p6, p7, p8, p9){ + //motors(motorsin) { + //predictthread(predictloopwrapper, this), + //predictticker( SIGTICKARGS(predictthread, 0x1) ), //(tfuncptr_t) (&Signalsetter::callback), osTimerPeriodic, (void*)(new Signalsetter(predictthread, 0x1)) ), + //sonarthread(sonarloopwrapper, this), + //sonarticker( SIGTICKARGS(sonarthread, 0x1) ){ + //updatethread(updateloopwrapper, this) { + + + //Initilising matrices + /* + X << 500 + << 0 + << 0; + + P << 1000 << 0 << 0 + << 0 << 1000 << 0 + << 0 << 0 << 1000; + + Q << 1 << 0 << 0 //temporary matrix + << 0 << 1 << 0 + << 0 << 0 << 1; + + */ + R = 100; //Preliminary + + //attach callback + //sonararray.callbackobj = (DummyCT*)this; + //sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance)) &Kalman::runupdate; + + //attach predict ticker + //predictticker.attach(this, &Kalman::predictlauncher, 0.5); //20Hz for now + + //predictticker.start(5000); + + //predictthread = new Thread(predictloopwrapper, this); + +} + +/* +void Kalman::predictloop() { + while (1) { + Thread::signal_wait(0x1); + predict(); + } +} +*/ + +/* +void Kalman::predict() { + + static int lastleft = 0; + static int lastright = 0; + + int left = motors.encoderToDistance(motors.getEncoder1()) - lastleft; + int right = motors.encoderToDistance(motors.getEncoder2()) - lastright; + + lastleft += left; + lastright += right; + + float dxp, dyp; + + //The below calculation are in body frame (where +x is forward) + float thetap = (right - left)*PI / (2.0f * robotCircumference); + + if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error + float d = (right + left)/2.0f; + dxp = d*cos(thetap/2); + dyp = d*sin(thetap/2); + } else { //calculate circle arc + //float r = (right + left) / (4.0f * PI * thetap); + float r = (right+left) / (2.0f*thetap); + dxp = abs(r)*sin(thetap); + dyp = r - r*cos(thetap); + } + + statelock.lock(); + + //rotating to cartesian frame and updating state + X(1,1) += dxp * cos(X(3,1)) - dyp * sin(X(3,1)); + X(2,1) += dxp * sin(X(3,1)) + dyp * cos(X(3,1)); + X(3,1) += thetap; + + //Linearising F around X + Matrix F(3,3); + F << 1 << 0 << (dxp * -sin(X(3,1)) - dyp * cos(X(3,1))) + << 0 << 1 << (dxp * cos(X(3,1)) - dyp * sin(X(3,1))) + << 0 << 0 << 1; + + //Updating P + P = F * P * MatrixMath::Transpose(F) + Q; + + statelock.unlock(); +} + + +void Kalman::sonarloop() { + while (1) { + Thread::signal_wait(0x1); + sonararray.startRange(); + } +} + + +void Kalman::runupdate(int sonarid, float dist) { + measurmentdata* measured = NULL;//(measurmentdata*)measureMQ.alloc(); + if (measured) { + measured->mtype = (measurement_t)sonarid; + measured->value = dist; + int putret = NULL;//osStatus putret = measureMQ.put(measured); + if (putret) + printf("putting in MQ error code %#x\r\n", putret); + } else { + printf("MQalloc returned NULL ptr\r\n"); + } +} + +void Kalman::updateloop() { + + while (1) { + + osEvent evt = measureMQ.get(); + if (evt.status == osEventMail) { + + measurmentdata& measured = *(measurmentdata*)evt.value.p; + int sonarid = measured.mtype; //Note, may support more measurment types than sonar in the future! + float dist = measured.value; + + statelock.lock(); + + float rbx = X(1,1) - beaconpos[sonarid].x; + float rby = X(2,1) - beaconpos[sonarid].y; + + float expecdist = sqrt(rbx*rbx + rby*rby); + float Y = dist - expecdist; + + float dhdx = rbx / expecdist; + float dhdy = rby / expecdist; + Matrix H(1,3); + H << dhdx << dhdy << 0; + + Matrix PH = P * MatrixMath::Transpose(H); + float S = (H * PH)(1,1) + R; + Matrix K = PH * (1/S); + + //Updating state + X += K*Y; + + + Matrix I3(3,3); + I3 << 1 << 0 << 0 + << 0 << 1 << 0 + << 0 << 0 << 1; + P = (I3 - K * H) * P; + + statelock.unlock(); + + printf("u %.1f %.1f %.1f\r\n", X(1,1), X(2,1), X(3,1)); + } else { + printf("ERROR: in updateloop, code %#x", evt); + } + + } + +} +*/ \ No newline at end of file