Diff: Processes/Kalman/Kalman.cpp

Revision:

20:70d651156779

Parent:

19:4b993a9a156e

--- a/Processes/Kalman/Kalman.cpp	Sun Apr 07 19:26:07 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Tue Apr 09 15:33:36 2013 +0000
@@ -5,9 +5,10 @@
 #include "rtos.h"
 #include "math.h"
 #include "supportfuncs.h"
+#include "Encoder.h"
 //#include "globals.h"
 
-#include <tvmet/Matrix.h>
+#include "tvmet/Matrix.h"
 using namespace tvmet;
 
 
@@ -15,6 +16,11 @@
 namespace Kalman
 {
 
+Ticker predictticker;
+
+DigitalOut OLED4(LED4);
+DigitalOut OLED1(LED1);
+
 //State variables
 Matrix<float, 3, 1> X;
 Matrix<float, 3, 3> P;
@@ -23,7 +29,7 @@
 float RawReadings[maxmeasure+1];
 float IRpahseOffset;
 
-bool Kalman_init = 0;
+bool Kalman_inited = 0;
 
 struct measurmentdata {
     measurement_t mtype;
@@ -33,29 +39,41 @@
 
 Mail <measurmentdata, 16> measureMQ;
 
+Thread* predict_thread_ptr = NULL;
 
 
 //Note: this init function assumes that the robot faces east, theta=0, in the +x direction
 void KalmanInit()
 {
+    printf("kalmaninit \r\n");
+    
+    //WARNING: HARDCODED!
+    
     //solve for our position (assume perfect bias)
-    const float d = beaconpos[0].y - beaconpos[1].y;
-    const float i = beaconpos[0].y - beaconpos[2].y;
-    const float j = beaconpos[0].x - beaconpos[2].x;
-    float r1 = RawReadings[SONAR0];
+    const float d = beaconpos[2].y - beaconpos[1].y;
+    const float i = beaconpos[2].y - beaconpos[0].y;
+    const float j = beaconpos[2].x - beaconpos[0].x;
+    float r1 = RawReadings[SONAR2];
     float r2 = RawReadings[SONAR1];
-    float r3 = RawReadings[SONAR2];
+    float r3 = RawReadings[SONAR0];
+    
+    printf("ranges: 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", r1, r2, r3);
 
     float y_coor = (r1*r1-r2*r2+d*d)/(2*d);
     float x_coor = (r1*r1-r3*r3+i*i+j*j)/(2*j) - (i*y_coor)/j;
-
+    
+    //coordinate system hack (for now)
+    x_coor = beaconpos[2].x - x_coor;
+    y_coor = beaconpos[2].y - y_coor;
+    
+    printf("solved pos from sonar: %f, %f \r\n", x_coor, y_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);
+    printf("IR 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
 
     float IR_Offsets[3];
     float fromb0offset = 0;
@@ -66,10 +84,10 @@
 
         //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
         IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
-        
+
         fromb0offset += constrainAngle(IR_Offsets[i] - IR_Offsets[0]);
     }
-    
+
     IRpahseOffset = constrainAngle(IR_Offsets[0] + fromb0offset/3);
 
     //debug
@@ -80,14 +98,24 @@
     X(1,0) = y_coor;
     X(2,0) = 0;
     statelock.unlock();
+    
+    Kalman_inited = 1;
 }
 
-/*
-void Kalman::predictloop(void* dummy)
+
+State getState(){
+    statelock.lock();
+    State state = {X(0,0), X(1,0), X(2,0)};
+    statelock.unlock();
+    return state;
+}
+
+
+void predictloop(void const *dummy)
 {
 
-    OLED4 = !ui.regid(0, 3);
-    OLED4 = !ui.regid(1, 4);
+    //OLED4 = !ui.regid(0, 3);
+    //OLED4 = !ui.regid(1, 4);
 
     float lastleft = 0;
     float lastright = 0;
@@ -96,11 +124,11 @@
         Thread::signal_wait(0x1);
         OLED1 = !OLED1;
 
-        int leftenc = encoders.getEncoder1();
-        int rightenc = encoders.getEncoder2();
+        float leftenc = left_encoder.getTicks() * ENCODER_M_PER_TICK;
+        float rightenc = right_encoder.getTicks() * ENCODER_M_PER_TICK;
 
-        float dleft = encoders.encoderToDistance(leftenc-lastleft)/1000.0f;
-        float dright = encoders.encoderToDistance(rightenc-lastright)/1000.0f;
+        float dleft = leftenc-lastleft;
+        float dright = rightenc-lastright;
 
         lastleft = leftenc;
         lastright = rightenc;
@@ -108,8 +136,8 @@
 
         //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
+        float thetap = (dright - dleft) / ENCODER_WHEELBASE;
+        if (abs(thetap) < 0.01f) { //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);
@@ -117,20 +145,20 @@
         } else { //calculate circle arc
             //float r = (right + left) / (4.0f * PI * thetap);
             r = (dright + dleft) / (2.0f*thetap);
-            dxp = abs(r)*sin(thetap);
+            dxp = r*sin(thetap);
             dyp = r - r*cos(thetap);
         }
 
         statelock.lock();
 
-        float tempX2 = X(2);
+        float tempX2 = X(2,0);
         //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);
+        X(0,0) += dxp * cos(X(2,0)) - dyp * sin(X(2,0));
+        X(1,0) += dxp * sin(X(2,0)) + dyp * cos(X(2,0));
+        X(2,0) = constrainAngle(X(2,0) + thetap);
 
         //Linearising F around X
-        float avgX2 = (X(2) + tempX2)/2.0f;
+        float avgX2 = (X(2,0) + 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)),
@@ -139,15 +167,15 @@
         //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;
+        float varxydt = xyvarpertime * KALMAN_PREDICT_PERIOD;
+        float varangdt = angvarpertime * KALMAN_PREDICT_PERIOD;
 
         //Rotating into cartesian frame
         Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
         Qsub = varfwd + varxydt, 0,
         0, varxydt;
 
-        Qrot = Rotmatrix(X(2));
+        Qrot = Rotmatrix(X(2,0));
 
         Qsubrot = Qrot * Qsub * trans(Qrot);
 
@@ -159,24 +187,42 @@
 
         P = F * P * trans(F) + Q;
 
+        //printf("x: %f, y: %f, t: %f\r\n", X(0,0), X(1,0), X(2,0));
         //Update UI
-        float statecpy[] = {X(0), X(1), X(2)};
-        ui.updateval(0, statecpy, 3);
+        //float statecpy[] = {X(0,0), X(1,0), X(2,0)};
+        //ui.updateval(0, statecpy, 3);
 
-        float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
-        ui.updateval(1, Pcpy, 4);
+        //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)
+
+void predict_event_setter(){
+    if(predict_thread_ptr)
+        predict_thread_ptr->signal_set(0x1);
+    else
+        OLED4 = 1;
+}
+
+void start_predict_ticker(Thread* predict_thread_ptr_in){
+    predict_thread_ptr = predict_thread_ptr_in;
+    predictticker.attach(predict_event_setter, KALMAN_PREDICT_PERIOD);
+}
+
+void runupdate(measurement_t type, float value, float variance)
 {
-    if (!Kalman_init)
+    if (!Kalman_inited) {
         RawReadings[type] = value;
-    else {
+    } else {
 
-        RawReadings[type] = value - SensorOffsets[type];
+        if (type >= IR0 && type <= IR2)
+            RawReadings[type] = value - IRpahseOffset;
+        else
+            RawReadings[type] = value;
+
 
         measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
         if (measured) {
@@ -185,19 +231,20 @@
             measured->variance = variance;
 
             osStatus putret = measureMQ.put(measured);
-            if (putret)
-                OLED4 = 1;
+            //if (putret)
+                //OLED4 = 1;
             //    printf("putting in MQ error code %#x\r\n", putret);
         } else {
-            OLED4 = 1;
+            //OLED4 = 1;
             //printf("MQalloc returned NULL ptr\r\n");
         }
-
+    
     }
+    
 
 }
-
-void Kalman::updateloop(void* dummy)
+/*
+void Kalman::updateloop(void const *dummy)
 {
 
     //sonar Y chanels
@@ -320,37 +367,6 @@
 
 }
 
-// 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;
-
-    P = 0.05, 0, 0,
-    0, 0.05, 0,
-    0, 0, 0.04;
-
-    // unlocks mutexes
-    statelock.unlock();
-
-}
-
 */
 
 } //Kalman Namespace
-