Kalman.cpp

00001 //***************************************************************************************
00002 //Kalman Filter implementation
00003 //***************************************************************************************
00004 #include "Kalman.h"
00005 #include "RFSRF05.h"
00006 #include "MatrixMath.h "
00007 #include "Matrix.h"
00008 #include "math.h"
00009 
00010 #define PI 3.14159265
00011 
00012 Kalman::Kalman() :
00013     X(3,1),
00014     P(3,3),
00015     Q(3,3),
00016     sonararray(p13,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9)  {
00017 
00018     //Initilising matrices
00019     X << 0
00020       << 0
00021       << 0;
00022       
00023     //Q << (insert numbers here)
00024     //R = a number;
00025     
00026     //attach callback
00027     sonararray.callbackobj = (DummyCT*)this;
00028     sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance)) &Kalman::update;
00029     
00030 }
00031 
00032 void Kalman::predict(float left, float right) {
00033 
00034     float dxp, dyp;
00035     
00036     //The below calculation are in body frame (where +x is forward)
00037     float thetap = (right - left) / (2.0f * PI * wheelbase);
00038 
00039     if (thetap < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
00040         float d = (right + left)/2.0f;
00041         dxp = d*cos(thetap/2);
00042         dyp = d*sin(thetap/2);
00043     } else { //calculate circle arc
00044         float r = (right + left) / (4.0f * PI * thetap);
00045         dxp = r*sin(thetap);
00046         dyp = r - r*cos(thetap);
00047     }
00048 
00049     //rotating to cartesian frame and updating state
00050     X(1,1) += dxp * cos(X(3,1)) - dyp * sin(X(3,1));
00051     X(2,1) += dxp * sin(X(3,1)) + dyp * cos(X(3,1));
00052     X(3,1) += thetap;
00053     
00054     //Linearising F around X
00055     Matrix F(3,3);
00056     F << 1 << 0 << (dxp * -sin(X(3,1)) - dyp * cos(X(3,1)))
00057       << 0 << 1 << (dxp * cos(X(3,1)) - dyp * sin(X(3,1)))
00058       << 0 << 0 << 1;
00059     
00060     //Updating P
00061     P = F * P * MatrixMath::Transpose(F) + Q;
00062 }
00063 
00064 void Kalman::update(int sonarid, float dist){
00065 
00066     float rbx = X(1,1) - beaconx;
00067     float rby = X(2,1) - beacony;
00068     
00069     float expecdist = sqrt(rbx*rbx + rby*rby);
00070     float Y = dist - expecdist;
00071     
00072     float dhdx = rbx / expecdist;
00073     float dhdy = rby / expecdist;
00074     Matrix H(1,3);
00075     H << dhdx << dhdy << 0;
00076     
00077     Matrix PH = P * MatrixMath::Transpose(H);    
00078     float S = (H * PH)(1,1) + R;
00079     Matrix K = PH * (1/S);
00080     
00081     //Updating state
00082     X += K*Y;
00083     
00084     Matrix I3(3,3);
00085     I3 << 1 << 0 << 0
00086        << 0 << 1 << 0
00087        << 0 << 0 << 1;
00088     P = (I3 - K * H) * P;
00089     
00090     
00091 
00092 }