Diff: Processes/Kalman/Kalman.cpp

Revision:

19:4b993a9a156e

Parent:

17:6263e90bf3ba

Child:

20:70d651156779

--- a/Processes/Kalman/Kalman.cpp	Sun Apr 07 17:51:59 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Sun Apr 07 19:26:07 2013 +0000
@@ -16,12 +16,12 @@
 {
 
 //State variables
-Vector<float, 3> X;
+Matrix<float, 3, 1> X;
 Matrix<float, 3, 3> P;
 Mutex statelock;
 
 float RawReadings[maxmeasure+1];
-float SensorOffsets[maxmeasure+1] = {0};
+float IRpahseOffset;
 
 bool Kalman_init = 0;
 
@@ -29,7 +29,7 @@
     measurement_t mtype;
     float value;
     float variance;
-}
+};
 
 Mail <measurmentdata, 16> measureMQ;
 
@@ -38,21 +38,15 @@
 //Note: this init function assumes that the robot faces east, theta=0, in the +x direction
 void KalmanInit()
 {
-
-    //Solving for sonar bias is done by entering the following into wolfram alpha
-    //(a-f)^2 = x^2 + y^2; (b-f)^2 = (x-3)^2 + y^2; (c-f)^2 = (x-1.5)^2+(y-2)^2: solve for x,y,f
-    //where a, b, c are the measured distances, and f is the bias
-
-    SensorOffsets[SONAR0] = sonartimebias;
-    SensorOffsets[SONAR1] = sonartimebias;
-    SensorOffsets[SONAR2] = sonartimebias;
-
     //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];
+    float r2 = RawReadings[SONAR1];
+    float r3 = RawReadings[SONAR2];
 
-    float y_coor = (r1*r1-r2*r2+d*d)/2d;
+    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;
 
     //IR
@@ -71,32 +65,24 @@
         float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
 
         //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
-        IR_Offset[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
+        IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
         
-        fromb0offset += IR_Offsets[i] - IR_Offset[0];
+        fromb0offset += constrainAngle(IR_Offsets[i] - IR_Offsets[0]);
     }
-
+    
+    IRpahseOffset = constrainAngle(IR_Offsets[0] + fromb0offset/3);
 
     //debug
-    printf("Offsets IR: %0.4f, Sonar: %0.4f \r\n",IR_Offset*180/PI,Sonar_Offset*1000 );
-
+    printf("Offsets IR: %0.4f\r\n",IRpahseOffset*180/PI);
 
     statelock.lock();
-    X(0) = x_coor/1000.0f;
-    X(1) = y_coor/1000.0f;
-
-    if (Colour)
-        X(2) = 0;
-    else
-        X(2) = PI;
+    X(0,0) = x_coor;
+    X(1,0) = y_coor;
+    X(2,0) = 0;
     statelock.unlock();
-
-
-    //reattach the IR processing
-    ir.attachisr();
 }
 
-
+/*
 void Kalman::predictloop(void* dummy)
 {
 
@@ -355,19 +341,6 @@
     X(0) = x_coor/1000.0f;
     X(1) = y_coor/1000.0f;
 
-
-
-    /*    if (Colour){
-            X(0) = 0.2;
-            X(1) = 0.2;
-            //X(2) = 0;
-            }
-        else {
-            X(0) = 2.8;
-            X(1) = 0.2;
-            //X(2) = PI;
-        }
-        */
     P = 0.05, 0, 0,
     0, 0.05, 0,
     0, 0, 0.04;
@@ -377,5 +350,7 @@
 
 }
 
+*/
+
 } //Kalman Namespace