Revision 48:254b124cef02, committed 2013-04-12

Comitter:

madcowswe

Date:

Fri Apr 12 17:03:53 2013 +0000

Parent:

47:fc471218af95

Child:

49:665bdca0f2cd

Commit message:

Online phase offset estimation, not working

Changed in this revision

--- a/Communication/coprocserial.cpp	Fri Apr 12 02:05:51 2013 +0000
+++ b/Communication/coprocserial.cpp	Fri Apr 12 17:03:53 2013 +0000
@@ -5,7 +5,7 @@
 Serial coprocserial(NC, P_SERIAL_RX);
 
 //DigitalOut OLED1(LED1);
-DigitalOut OLED3(LED3);
+//DigitalOut OLED3(LED3);
 
 // bytes packing
 typedef union {
@@ -52,7 +52,7 @@
         if (++ctr == 12){
             ctr = 0;
             
-            OLED3 = !OLED3;
+            //OLED3 = !OLED3;
             buffprintflag = 1;
             
             //runupdate

--- a/Processes/Kalman/Kalman.cpp	Fri Apr 12 02:05:51 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Fri Apr 12 17:03:53 2013 +0000
@@ -20,17 +20,17 @@
 Ticker predictticker;
 
 DigitalOut OLED4(LED4);
+DigitalOut OLED3(LED3);
 DigitalOut OLED1(LED1);
 DigitalOut OLED2(LED2);
 
 //State variables
-Matrix<float, 3, 1> X;
-Matrix<float, 3, 3> P;
+Matrix<float, 4, 1> X;
+Matrix<float, 4, 4> P;
 Mutex statelock;
 
 float RawReadings[maxmeasure+1];
 int sensorseenflags = 0;
-float IRphaseOffset;
 
 bool Kalman_inited = 0;
 
@@ -49,12 +49,12 @@
 void KalmanInit()
 {
     printf("kalmaninit \r\n");
-    
+
     //WARNING: HARDCODED! TODO: fix it so it works for both sides!
-    
+
     printf("waiting for all sonar, and at least 1 IR\r\n");
     while( ((sensorseenflags & 0x7)^0x7) || !(sensorseenflags & 0x7<<3) );
-    
+
     //solve for our position (assume perfect bias)
     const float d = beaconpos[2].y - beaconpos[1].y;
     const float i = beaconpos[2].y - beaconpos[0].y;
@@ -62,18 +62,18 @@
     float r1 = RawReadings[SONAR2];
     float r2 = RawReadings[SONAR1];
     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];
@@ -84,52 +84,55 @@
     float IR_Offsets[3] = {0};
     float frombrefoffset = 0;
     int refbeacon = 0;
-    
-    for (int i = 0; i < 3; i++){
-        if (sensorseenflags & 1<<(3+i)){
+
+    for (int i = 0; i < 3; i++) {
+        if (sensorseenflags & 1<<(3+i)) {
             refbeacon = i;
             break;
         }
     }
-    
+
     printf("refbeacon is %d\r\n", refbeacon);
-    
+
     int cnt = 0;
     for (int i = 0; i < 3; i++) {
 
-        if (sensorseenflags & 1<<(3+i)){
+        if (sensorseenflags & 1<<(3+i)) {
             cnt++;
-            
+
             //Compute IR offset
             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_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
-    
+
             frombrefoffset += constrainAngle(IR_Offsets[i] - IR_Offsets[refbeacon]);
         }
     }
 
-    IRphaseOffset = constrainAngle(IR_Offsets[refbeacon] + frombrefoffset/cnt);
+    X(3,0) = constrainAngle(IR_Offsets[refbeacon] + frombrefoffset/cnt);
 
     //debug
-    printf("Offsets IR: %0.4f\r\n",IRphaseOffset*180/PI);
+    printf("Offsets IR: %0.4f\r\n",X(3,0)*180/PI);
 
     statelock.lock();
-    X(0,0) = x_coor-TURRET_FWD_PLACEMENT; //assume facing east
+    X(0,0) = x_coor-TURRET_FWD_PLACEMENT; 
     X(1,0) = y_coor;
-    X(2,0) = 0;
-    
-    P = 0.02*0.02, 0, 0,
-        0, 0.02*0.02, 0,
-        0, 0, 0.04;
+    X(2,0) = 0; //TODO: assume facing east, need to account for both starting pos
+
+    P = 0.02*0.02,  0,          0,          0,
+        0,          0.02*0.02,  0,          0,
+        0,          0,          0.4*0.4,    -0.4*0.4,
+        0,          0,          -0.4*0.4,   0.4*0.4;
+        
     statelock.unlock();
-    
+
     Kalman_inited = 1;
 }
 
 
-State getState(){
+State getState()
+{
     statelock.lock();
     State state = {X(0,0), X(1,0), X(2,0)};
     statelock.unlock();
@@ -182,13 +185,14 @@
         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);
+        //X(3,0) += 0;
 
         //Linearising F around X
-        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)),
-        0, 0, 1;
+        Matrix<float, 4, 4> F;
+        F = 1, 0, (dxp * -sin(tempX2) - dyp * cos(tempX2)), 0,
+            0, 1, (dxp * cos(tempX2) - dyp * sin(tempX2)),  0,
+            0, 0, 1,                                        0,
+            0, 0, 0,                                        1;
 
         //Generating forward and rotational variance
         float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f;
@@ -206,10 +210,11 @@
         Qsubrot = Qrot * Qsub * trans(Qrot);
 
         //Generate Q
-        Matrix<float, 3, 3> Q;//(Qsubrot);
-        Q = Qsubrot(0,0), Qsubrot(0,1), 0,
-        Qsubrot(1,0), Qsubrot(1,1), 0,
-        0, 0, varang + varangdt;
+        Matrix<float, 4, 4> Q;//(Qsubrot);
+        Q = Qsubrot(0,0),   Qsubrot(0,1),   0,                  0,
+            Qsubrot(1,0),   Qsubrot(1,1),   0,                  0,
+            0,              0,              varang + varangdt,  0,
+            0,              0,              0,                  0;
 
         P = F * P * trans(F) + Q;
 
@@ -226,14 +231,16 @@
 }
 
 
-void predict_event_setter(){
+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){
+void start_predict_ticker(Thread* predict_thread_ptr_in)
+{
     predict_thread_ptr = predict_thread_ptr_in;
     predictticker.attach(predict_event_setter, KALMAN_PREDICT_PERIOD);
 }
@@ -242,16 +249,7 @@
 {
     sensorseenflags |= 1<<type;
 
-    if (!Kalman_inited) {
-        RawReadings[type] = value;
-    } else {
-
-        if (type >= IR0 && type <= IR2)
-            RawReadings[type] = value - IRphaseOffset;
-        else
-            RawReadings[type] = value;
-
-
+    if (Kalman_inited) {
         measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
         if (measured) {
             measured->mtype = type;
@@ -260,15 +258,15 @@
 
             osStatus putret = measureMQ.put(measured);
             //if (putret)
-                //OLED4 = 1;
+            //OLED4 = 1;
             //    printf("putting in MQ error code %#x\r\n", putret);
         } else {
             //OLED4 = 1;
             //printf("MQalloc returned NULL ptr\r\n");
         }
-    
+
     }
-    
+
 
 }
 
@@ -287,10 +285,10 @@
 
     bool aborton2stddev = false;
 
-    Matrix<float, 1, 3> H;
+    Matrix<float, 1, 4> H;
 
     float Y,S;
-    const Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
+    const Matrix<float, 4, 4> I4( identity< Matrix<float, 4, 4> >() );
 
 
     while (1) {
@@ -317,13 +315,13 @@
                     aborton2stddev = true;
 
                     statelock.lock();
-                    
+
                     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;
 
@@ -336,7 +334,7 @@
                     float dhdy = rby * r_expecdist;
                     float dhdt = fp_ct*dhdy - fp_st*dhdx;
 
-                    H = dhdx, dhdy, dhdt;
+                    H = dhdx, dhdy, dhdt, 0;
 
                 } else if (type <= IR2) {
 
@@ -344,14 +342,14 @@
                     int IRidx = type-3;
 
                     statelock.lock();
-                    
+
                     float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0));
                     float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0));
 
                     float brx = beaconpos[IRidx].x - (X(0,0) + fp_ct);
                     float bry = beaconpos[IRidx].y - (X(1,0) + fp_st);
 
-                    float expecang = atan2(bry, brx) - X(2,0); //constrainAngle can be called late
+                    float expecang = atan2(bry, brx) - X(2,0) + X(3,0); //constrainAngle can be called late
                     Y = constrainAngle(value - expecang);
 
                     //send to ui
@@ -361,24 +359,28 @@
                     float dhdx = -bry*r_dstsq;
                     float dhdy = brx*r_dstsq;
                     float dhdt = fp_ct*dhdy - fp_st*dhdx - 1.0f;
-                    H = dhdx, dhdy, dhdt;
+                    float dhdp = 1;
+                    H = dhdx, dhdy, dhdt, dhdp;
                 }
 
-                Matrix<float, 3, 1> PH (P * trans(H));
-                S = (H * PH)(0,0) + variance*10; //TODO: temp hack
+                Matrix<float, 4, 1> PHt (P * trans(H));
+                S = (H * PHt)(0,0) + variance;
 
+                OLED3 = 0;
                 if (aborton2stddev && Y*Y > 4 * S) {
+                    OLED3 = 1;
                     statelock.unlock();
                     continue;
                 }
 
-                Matrix<float, 3, 1> K (PH * (1/S));
+                Matrix<float, 4, 1> K (PHt * (1/S));
 
                 //Updating state
                 X += K * Y;
                 X(2,0) = constrainAngle(X(2,0));
+                X(3,0) = constrainAngle(X(3,0));
 
-                P = (I3 - K * H) * P;
+                P = (I4 - K * H) * P;
 
                 statelock.unlock();
 

--- a/Processes/Kalman/Kalman.h	Fri Apr 12 02:05:51 2013 +0000
+++ b/Processes/Kalman/Kalman.h	Fri Apr 12 17:03:53 2013 +0000
@@ -23,7 +23,6 @@
 void runupdate(measurement_t type, float value, float variance);
 
 extern float RawReadings[maxmeasure+1];
-extern float IRphaseOffset;
 
 extern bool Kalman_inited;
 

--- a/main.cpp	Fri Apr 12 02:05:51 2013 +0000
+++ b/main.cpp	Fri Apr 12 17:03:53 2013 +0000
@@ -75,8 +75,8 @@
     
     Thread updatethread(Kalman::updateloop, NULL, osPriorityNormal, 2084);
     
-    Ticker motorcontroltestticker;
-    motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.01);
+    //Ticker motorcontroltestticker;
+    //motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.01);
 
     // ai layer thread
     Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);