Diff: Kalman/Kalman.cpp

Revision:

1:bbabbd997d21

Parent:

0:f3bf6c7e2283

--- a/Kalman/Kalman.cpp	Fri Apr 20 20:39:35 2012 +0000
+++ b/Kalman/Kalman.cpp	Fri Apr 20 21:56:15 2012 +0000
@@ -10,6 +10,7 @@
 #include "globals.h"
 #include "motors.h"
 #include "system.h"
+#include "geometryfuncs.h"
 
 #include <tvmet/Matrix.h>
 #include <tvmet/Vector.h>
@@ -27,7 +28,7 @@
         predictticker( SIGTICKARGS(predictthread, 0x1) ),
 //        sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
 //        sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
-        updatethread(updateloopwrapper, this, osPriorityNormal, 512) {
+        updatethread(updateloopwrapper, this, osPriorityNormal, 2048) {
 
     //Initilising matrices
 
@@ -38,9 +39,9 @@
         0, 1, 0,
         0, 0, 0.04;
 
-    Q = 0.002, 0, 0, //temporary matrix, Use dt!
-        0, 0.002, 0,
-        0, 0, 0.002;
+    //Q = 0.002, 0, 0, //temporary matrix, Use dt!
+    //    0, 0.002, 0,
+    //    0, 0, 0.002;
 
     //measurment variance R is provided by each sensor when calling runupdate
 
@@ -49,7 +50,7 @@
     sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance, float variance)) &Kalman::runupdate;
 
 
-//    predictticker.start(20);
+    predictticker.start(20);
 //    sonarticker.start(50);
 
 
@@ -57,31 +58,27 @@
 
 
 void Kalman::predictloop() {
-    Matrix<float, 3, 3> F;
-    int lastleft = motors.getEncoder1();
-    int lastright = motors.getEncoder2();
-    int leftenc;
-    int rightenc;
-    float dleft,dright;
-    float dxp, dyp;
-    float thetap,d,r;
+
+    float lastleft = 0;
+    float lastright = 0;
 
     while (1) {
-//        Thread::signal_wait(0x1);
+        Thread::signal_wait(0x1);
         led1 = !led1;
         
-        leftenc = motors.getEncoder1();
-        rightenc = motors.getEncoder2();
+        int leftenc = motors.getEncoder1();
+        int rightenc = motors.getEncoder2();
         
-        dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f;
-        dright = motors.encoderToDistance(rightenc-lastright)/1000.0f;
+        float dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f;
+        float dright = motors.encoderToDistance(rightenc-lastright)/1000.0f;
 
         lastleft = leftenc;
         lastright = rightenc;
 
 
         //The below calculation are in body frame (where +x is forward)
-        thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f);
+        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
             d = (dright + dleft)/2.0f;
             dxp = d*cos(thetap/2.0f);
@@ -102,15 +99,36 @@
         X(2) = rectifyAng(X(2) + thetap);
 
         //Linearising F around X
+        Matrix<float, 3, 3> F;
         F = 1, 0, (dxp * -sin(X(2)) - dyp * cos(X(2))),
             0, 1, (dxp * cos(X(2)) - dyp * sin(X(2))),
             0, 0, 1;
 
+        //Generating forward and rotational variance
+        float varfwd = fwdvarperunit * (dright + dleft) / 2.0f;
+        float varang = varperang * thetap;
+        float varxydt = xyvarpertime * PREDICTPERIOD;
+        float varangdt = angvarpertime * PREDICTPERIOD;
+        
+        //Rotating into cartesian frame
+        Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
+        Qsub = varfwd + varxydt, 0,
+               0, varxydt;
+               
+        Qrot = Rotmatrix(X(2));
+               
+        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;
 
         P = F * P * trans(F) + Q;
 
         statelock.unlock();
-        Thread::wait(20);
+        //Thread::wait(PREDICTPERIOD);
 
         //cout << "predict" << X << endl;
         //cout << P << endl;
@@ -179,7 +197,7 @@
 
                     float dist = value / 1000.0f; //converting to m from mm
                     int sonarid = type;
-                    aborton2stddev = true;
+                    aborton2stddev = false; 
 
                     statelock.lock();
                     SonarMeasures[sonarid] = dist; //update the current sonar readings
@@ -187,7 +205,7 @@
                     rbx = X(0) - beaconpos[sonarid].x/1000.0f;
                     rby = X(1) - beaconpos[sonarid].y/1000.0f;
 
-                    expecdist = sqrt(rbx*rbx + rby*rby);
+                    expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
                     Y = dist - expecdist;
 
                     dhdx = rbx / expecdist;
@@ -196,7 +214,8 @@
                     H = dhdx, dhdy, 0;
 
                 } else if (type <= IR3) {
-                    
+                  
+                    aborton2stddev = false;
                     int IRidx = type-3;
                     
                     statelock.lock();
@@ -206,11 +225,11 @@
                     rby = X(1) - beaconpos[IRidx].y/1000.0f;
                     
                     float expecang = atan2(-rbx, -rby) - X(2);
-                    Y = rectifyAng(value - expecang);
+                    //printf("expecang: %0.4f, value: %0.4f \n\r", expecang*180/PI,value*180/PI);
+                    Y = rectifyAng(value + expecang);
                     
                     float dstsq = rbx*rbx + rby*rby;
                     H = -rby/dstsq, rbx/dstsq, -1;
-                    
                 }
 
                 Matrix<float, 3, 1> PH (P * trans(H));