Diff: Processes/Kalman/Kalman.cpp

Revision:

27:7cb3a21d9a2e

Parent:

23:6e3218cf75f8

Child:

28:664e81033846

--- a/Processes/Kalman/Kalman.cpp	Wed Apr 10 02:01:51 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Wed Apr 10 03:46:23 2013 +0000
@@ -21,6 +21,7 @@
 
 DigitalOut OLED4(LED4);
 DigitalOut OLED1(LED1);
+DigitalOut OLED2(LED2);
 
 //State variables
 Matrix<float, 3, 1> X;
@@ -95,7 +96,7 @@
     printf("Offsets IR: %0.4f\r\n",IRphaseOffset*180/PI);
 
     statelock.lock();
-    X(0,0) = x_coor;
+    X(0,0) = x_coor-TURRET_FWD_PLACEMENT; //assume facing east
     X(1,0) = y_coor;
     X(2,0) = 0;
     
@@ -248,29 +249,26 @@
     
 
 }
-/*
+
 void Kalman::updateloop(void const*)
 {
 
     //sonar Y chanels
-    ui.regid(2, 1);
-    ui.regid(3, 1);
-    ui.regid(4, 1);
+    OLED4 = !Printing::registerID(2, 1);
+    OLED4 = !Printing::registerID(3, 1);
+    OLED4 = !Printing::registerID(4, 1);
 
     //IR Y chanels
-    ui.regid(5, 1);
-    ui.regid(6, 1);
-    ui.regid(7, 1);
+    OLED4 = !Printing::registerID(5, 1);
+    OLED4 = !Printing::registerID(6, 1);
+    OLED4 = !Printing::registerID(7, 1);
 
-    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> >() );
+    float Y,S;
+    const Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
 
 
     while (1) {
@@ -281,66 +279,67 @@
         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;
+            measurement_t type = measured.mtype; //Note, may support more measurment types than sonar in the future!
+            float value = measured.value;
+            float variance = measured.variance;
 
             // don't forget to free the memory
             measureMQ.free(&measured);
 
             if (type <= maxmeasure) {
 
-                if (type <= SONAR3) {
+                if (type <= SONAR2) {
 
-                    InitLock.lock();
-                    float dist = value / 1000.0f - Sonar_Offset; //converting to m from mm,subtract the offset
-                    InitLock.unlock();
-
+                    float dist = value;
                     int sonarid = type;
                     aborton2stddev = true;
 
                     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);
+                    
+                    float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0));
+                    float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0));
+                    
+                    float rbx = X(0,0) + fp_ct - beaconpos[sonarid].x;
+                    float rby = X(1,0) + fp_st - beaconpos[sonarid].y;
+                    
+                    float expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
                     Y = dist - expecdist;
 
                     //send to ui
-                    ui.updateval(sonarid+2, Y);
+                    Printing::updateval(sonarid+2, Y);
 
-                    dhdx = rbx / expecdist;
-                    dhdy = rby / expecdist;
+                    float r_expecdist = 1.0f/expecdist;
 
-                    H = dhdx, dhdy, 0;
+                    float dhdx = rbx * r_expecdist;
+                    float dhdy = rby * r_expecdist;
+                    float dhdt = (fp_ct*rby - fp_st*rbx) * r_expecdist;
 
-                } else if (type <= IR3) {
+                    H = dhdx, dhdy, dhdt;
 
-                    aborton2stddev = false;
+                } else if (type <= IR2) {
+
+                    aborton2stddev = true;
                     int IRidx = type-3;
 
-                    // subtract the IR offset
-                    InitLock.lock();
-                    value -= IR_Offset;
-                    InitLock.unlock();
+                    statelock.lock();
+                    
+                    float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0));
+                    float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0));
 
-                    statelock.lock();
-                    IRMeasures[IRidx] = value;
+                    float brx = beaconpos[IRidx].x - (X(0,0) + fp_ct);
+                    float bry = beaconpos[IRidx].y - (X(1,0) + fp_st);
 
-                    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 expecang = atan2(bry, brx) - X(2,0); //constrainAngle can be called late
+                    Y = constrainAngle(value - expecang);
 
                     //send to ui
-                    ui.updateval(IRidx + 5, Y);
+                    Printing::updateval(IRidx + 5, Y);
 
-                    float dstsq = rbx*rbx + rby*rby;
-                    H = -rby/dstsq, rbx/dstsq, -1;
+                    float r_dstsq = 1.0f/(brx*brx + bry*bry);
+                    float dhdx = -bry*r_dstsq;
+                    float dhdy = brx*r_dstsq;
+                    float dhdt = fp_ct*dhdy - fp_st*dhdx - 1.0f;
+                    H = dhdx, dhdy, dhdt;
                 }
 
                 Matrix<float, 3, 1> PH (P * trans(H));
@@ -354,8 +353,8 @@
                 Matrix<float, 3, 1> K (PH * (1/S));
 
                 //Updating state
-                X += col(K, 0) * Y;
-                X(2) = rectifyAng(X(2));
+                X += K * Y;
+                X(2,0) = constrainAngle(X(2,0));
 
                 P = (I3 - K * H) * P;
 
@@ -372,6 +371,5 @@
 
 }
 
-*/
 
 } //Kalman Namespace