Diff: Processes/Kalman/Kalman.cpp

Revision:

17:6263e90bf3ba

Parent:

16:52250d8d8fce

Child:

19:4b993a9a156e

--- a/Processes/Kalman/Kalman.cpp	Sun Apr 07 16:50:36 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Sun Apr 07 17:51:59 2013 +0000
@@ -38,46 +38,44 @@
 //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)
-    float x_coor = 
-    
-    
+    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 y_coor = (r1*r1-r2*r2+d*d)/2d;
+    float x_coor = (r1*r1-r3*r3+i*i+j*j)/(2*j) - (i*y_coor)/j;
+
     //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);
 
-    IR_Offset = 0;
+    float IR_Offsets[3];
+    float fromb0offset = 0;
     for (int i = 0; i < 3; i++) {
 
         //Compute IR offset
         float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
-        if (!Colour)
-            angle_est -= PI;
+
         //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
+        IR_Offset[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
         
-        // take average offset angle from valid readings
-        if (IRMeasuresloc[i] != 0) {
-            // changed to current angle - estimated angle
-            float angle_temp = IRMeasuresloc[i] - angle_est;
-            angle_temp -= (floor(angle_temp/(2*PI)))*2*PI;
-            IR_Offset += angle_temp;
-        }
+        fromb0offset += IR_Offsets[i] - IR_Offset[0];
     }
-    IR_Offset /= float(beacon_cnt);
+
 
     //debug
     printf("Offsets IR: %0.4f, Sonar: %0.4f \r\n",IR_Offset*180/PI,Sonar_Offset*1000 );
@@ -191,9 +189,9 @@
     if (!Kalman_init)
         RawReadings[type] = value;
     else {
-        
+
         RawReadings[type] = value - SensorOffsets[type];
-        
+
         measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
         if (measured) {
             measured->mtype = type;