Kalman filter for Eurobot
Kalman.cpp
- Committer:
- madcowswe
- Date:
- 2012-03-20
- Revision:
- 0:a0285293f6a6
File content as of revision 0:a0285293f6a6:
//*************************************************************************************** //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; }