Diff: Kalman.cpp

Revision:

0:a0285293f6a6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Kalman.cpp	Tue Mar 20 12:43:16 2012 +0000
@@ -0,0 +1,92 @@
+//***************************************************************************************
+//Kalman Filter implementation
+//***************************************************************************************
+#include "Kalman.h"
+#include "RFSRF05.h"
+#include "MatrixMath.h"
+#include "Matrix.h"
+#include "math.h"
+
+#define PI 3.14159265
+
+Kalman::Kalman() :
+    X(3,1),
+    P(3,3),
+    Q(3,3),
+    sonararray(p13,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9)  {
+
+    //Initilising matrices
+    X << 0
+      << 0
+      << 0;
+      
+    //Q << (insert numbers here)
+    //R = a number;
+    
+    //attach callback
+    sonararray.callbackobj = (DummyCT*)this;
+    sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance)) &Kalman::update;
+    
+}
+
+void Kalman::predict(float left, float right) {
+
+    float dxp, dyp;
+    
+    //The below calculation are in body frame (where +x is forward)
+    float thetap = (right - left) / (2.0f * PI * wheelbase);
+
+    if (thetap < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
+        float d = (right + left)/2.0f;
+        dxp = d*cos(thetap/2);
+        dyp = d*sin(thetap/2);
+    } else { //calculate circle arc
+        float r = (right + left) / (4.0f * PI * thetap);
+        dxp = r*sin(thetap);
+        dyp = r - r*cos(thetap);
+    }
+
+    //rotating to cartesian frame and updating state
+    X(1,1) += dxp * cos(X(3,1)) - dyp * sin(X(3,1));
+    X(2,1) += dxp * sin(X(3,1)) + dyp * cos(X(3,1));
+    X(3,1) += thetap;
+    
+    //Linearising F around X
+    Matrix F(3,3);
+    F << 1 << 0 << (dxp * -sin(X(3,1)) - dyp * cos(X(3,1)))
+      << 0 << 1 << (dxp * cos(X(3,1)) - dyp * sin(X(3,1)))
+      << 0 << 0 << 1;
+    
+    //Updating P
+    P = F * P * MatrixMath::Transpose(F) + Q;
+}
+
+void Kalman::update(int sonarid, float dist){
+
+    float rbx = X(1,1) - beaconx;
+    float rby = X(2,1) - beacony;
+    
+    float expecdist = sqrt(rbx*rbx + rby*rby);
+    float Y = dist - expecdist;
+    
+    float dhdx = rbx / expecdist;
+    float dhdy = rby / expecdist;
+    Matrix H(1,3);
+    H << dhdx << dhdy << 0;
+    
+    Matrix PH = P * MatrixMath::Transpose(H);    
+    float S = (H * PH)(1,1) + R;
+    Matrix K = PH * (1/S);
+    
+    //Updating state
+    X += K*Y;
+    
+    Matrix I3(3,3);
+    I3 << 1 << 0 << 0
+       << 0 << 1 << 0
+       << 0 << 0 << 1;
+    P = (I3 - K * H) * P;
+    
+    
+
+}
\ No newline at end of file