Diff: Kalman/Kalman.cpp

Revision:

0:349dc9b0984f

--- /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