Kalman filter for Eurobot
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