Diff: Kalman.cpp
- Revision:
- 0:e238496b8073
- Child:
- 1:4964fa534202
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman.cpp Thu Apr 26 19:11:11 2012 +0000
@@ -0,0 +1,342 @@
+//***************************************************************************************
+//Kalman Filter implementation
+//***************************************************************************************
+#include "Kalman.h"
+#include "rtos.h"
+#include "RFSRF05.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;
+
+Kalman::Kalman(Motors &motorsin) :
+ ir(*this),
+ 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, 512) {
+
+ Kalman_init = false;
+ //Intialising some arrays to zero
+ for (int kk = 0; kk < 3; kk ++) {
+ SonarMeasure_Offset[kk] = 0;
+ }
+ //Initialising other vars
+
+
+ //Initilising matrices
+
+ // X = x, y, theta;
+ X = 0.5, 0, 0;
+
+ P = 1, 0, 0,
+ 0, 1, 0,
+ 0, 0, 0.04;
+
+ //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);
+
+}
+
+
+//Note: this init function assumes that the robot faces east, theta=0, in the +x direction
+void Kalman::KalmanInit() {
+ float SonarMeasuresx1000[3];
+ float IRMeasuresloc[3];
+ int beacon_cnt = 0;
+ // set initiating flag to false
+ Kalman_init = false;
+
+ // init the offset array
+ for (int k = 0; k < 3; k ++) {
+ SonarMeasure_Offset[k] = 0;
+ IRMeasures[k] = 0;
+ }
+
+ LPC_UART0->FCR = LPC_UART0->FCR | 0x06; // Flush the serial FIFO buffer / OR with FCR
+ //wating untill the IR has reved up and picked up some data
+ wait(1);
+
+ //temporaraly disable IR updates
+ ir.detachisr();
+ //IRturret.attach(NULL,Serial::RxIrq);
+
+ //lock the state throughout the computation, as we will override the state at the end
+ statelock.lock();
+
+ SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
+ SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
+ SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
+ IRMeasuresloc[0] = IRMeasures[0];
+ IRMeasuresloc[1] = IRMeasures[1];
+ IRMeasuresloc[2] = IRMeasures[2];
+ //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 y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
+ float x_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
+
+ //Compute sonar offset
+ float Dist_Exp[3];
+ for (int k = 0; k < 3; k++) {
+ Dist_Exp[k] = sqrt((beaconpos[k].y - y_coor)*(beaconpos[k].y - y_coor)+(beaconpos[k].x - x_coor)*(beaconpos[k].x - x_coor));
+ SonarMeasure_Offset[k] = (SonarMeasuresx1000[k]-Dist_Exp[k])/1000.0f;
+ }
+
+ //Compute IR offset
+ ir.angleOffset = 0;
+ for (int i = 0; i < 3; i++) {
+ float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
+ // take average offset angle from valid readings
+ if (IRMeasuresloc[i] != 0) {
+ beacon_cnt ++;
+ // changed to current angle - estimated angle
+ float angle_temp = IRMeasuresloc[i] - angle_est;
+ angle_temp -= (floor(angle_temp/(2*PI)))*2*PI;
+ ir.angleOffset += angle_temp;
+ }
+ }
+ ir.angleOffset = ir.angleOffset/float(beacon_cnt);
+ //printf("\n\r");
+
+ //statelock already locked
+ ir.angleInit = true;
+ // set int flag to true
+ Kalman_init = true;
+ X(0) = x_coor/1000.0f;
+ X(1) = y_coor/1000.0f;
+ X(2) = 0;
+ statelock.unlock();
+
+ //printf("x: %0.4f, y: %0.4f, offset: %0.4f \n\r", x_coor, y_coor, angleOffset*180/PI);
+
+ //reattach the IR processing
+ ir.attachisr();
+ //IRturret.attach(&IR::vIRValueISR,Serial::RxIrq);
+}
+
+
+void Kalman::predictloop() {
+
+ float lastleft = 0;
+ float lastright = 0;
+
+ while (1) {
+ Thread::signal_wait(0x1);
+ OLED1 = !OLED1;
+
+ 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)
+ 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() {
+ 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) {
+
+ float dist = value / 1000.0f; //converting to m from mm
+ int sonarid = type;
+ aborton2stddev = false;
+
+ // Remove the offset if possible
+ if (Kalman_init)
+ dist = dist - SonarMeasure_Offset[sonarid];
+
+ 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;
+
+ 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(-rby, -rbx) - X(2);
+ 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 {
+ OLED4 = 1;
+ //printf("ERROR: in updateloop, code %#x", evt);
+ }
+
+ }
+
+}
\ No newline at end of file
Shuto Naruse