Oskar Weigl / Kalman

Kalman filter for Eurobot

Kalman.cpp@0:a0285293f6a6, 2012-03-20 (annotated)

Committer:
madcowswe
Date:
Tue Mar 20 12:43:16 2012 +0000
Revision:
0:a0285293f6a6
Algo done, vars not done

Who changed what in which revision?

UserRevisionLine numberNew contents of line
madcowswe 0:a0285293f6a6 1 //***************************************************************************************
madcowswe 0:a0285293f6a6 2 //Kalman Filter implementation
madcowswe 0:a0285293f6a6 3 //***************************************************************************************
madcowswe 0:a0285293f6a6 4 #include "Kalman.h"
madcowswe 0:a0285293f6a6 5 #include "RFSRF05.h"
madcowswe 0:a0285293f6a6 6 #include "MatrixMath.h"
madcowswe 0:a0285293f6a6 7 #include "Matrix.h"
madcowswe 0:a0285293f6a6 8 #include "math.h"
madcowswe 0:a0285293f6a6 9
madcowswe 0:a0285293f6a6 10 #define PI 3.14159265
madcowswe 0:a0285293f6a6 11
madcowswe 0:a0285293f6a6 12 Kalman::Kalman() :
madcowswe 0:a0285293f6a6 13 X(3,1),
madcowswe 0:a0285293f6a6 14 P(3,3),
madcowswe 0:a0285293f6a6 15 Q(3,3),
madcowswe 0:a0285293f6a6 16 sonararray(p13,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9) {
madcowswe 0:a0285293f6a6 17
madcowswe 0:a0285293f6a6 18 //Initilising matrices
madcowswe 0:a0285293f6a6 19 X << 0
madcowswe 0:a0285293f6a6 20 << 0
madcowswe 0:a0285293f6a6 21 << 0;
madcowswe 0:a0285293f6a6 22
madcowswe 0:a0285293f6a6 23 //Q << (insert numbers here)
madcowswe 0:a0285293f6a6 24 //R = a number;
madcowswe 0:a0285293f6a6 25
madcowswe 0:a0285293f6a6 26 //attach callback
madcowswe 0:a0285293f6a6 27 sonararray.callbackobj = (DummyCT*)this;
madcowswe 0:a0285293f6a6 28 sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance)) &Kalman::update;
madcowswe 0:a0285293f6a6 29
madcowswe 0:a0285293f6a6 30 }
madcowswe 0:a0285293f6a6 31
madcowswe 0:a0285293f6a6 32 void Kalman::predict(float left, float right) {
madcowswe 0:a0285293f6a6 33
madcowswe 0:a0285293f6a6 34 float dxp, dyp;
madcowswe 0:a0285293f6a6 35
madcowswe 0:a0285293f6a6 36 //The below calculation are in body frame (where +x is forward)
madcowswe 0:a0285293f6a6 37 float thetap = (right - left) / (2.0f * PI * wheelbase);
madcowswe 0:a0285293f6a6 38
madcowswe 0:a0285293f6a6 39 if (thetap < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
madcowswe 0:a0285293f6a6 40 float d = (right + left)/2.0f;
madcowswe 0:a0285293f6a6 41 dxp = d*cos(thetap/2);
madcowswe 0:a0285293f6a6 42 dyp = d*sin(thetap/2);
madcowswe 0:a0285293f6a6 43 } else { //calculate circle arc
madcowswe 0:a0285293f6a6 44 float r = (right + left) / (4.0f * PI * thetap);
madcowswe 0:a0285293f6a6 45 dxp = r*sin(thetap);
madcowswe 0:a0285293f6a6 46 dyp = r - r*cos(thetap);
madcowswe 0:a0285293f6a6 47 }
madcowswe 0:a0285293f6a6 48
madcowswe 0:a0285293f6a6 49 //rotating to cartesian frame and updating state
madcowswe 0:a0285293f6a6 50 X(1,1) += dxp * cos(X(3,1)) - dyp * sin(X(3,1));
madcowswe 0:a0285293f6a6 51 X(2,1) += dxp * sin(X(3,1)) + dyp * cos(X(3,1));
madcowswe 0:a0285293f6a6 52 X(3,1) += thetap;
madcowswe 0:a0285293f6a6 53
madcowswe 0:a0285293f6a6 54 //Linearising F around X
madcowswe 0:a0285293f6a6 55 Matrix F(3,3);
madcowswe 0:a0285293f6a6 56 F << 1 << 0 << (dxp * -sin(X(3,1)) - dyp * cos(X(3,1)))
madcowswe 0:a0285293f6a6 57 << 0 << 1 << (dxp * cos(X(3,1)) - dyp * sin(X(3,1)))
madcowswe 0:a0285293f6a6 58 << 0 << 0 << 1;
madcowswe 0:a0285293f6a6 59
madcowswe 0:a0285293f6a6 60 //Updating P
madcowswe 0:a0285293f6a6 61 P = F * P * MatrixMath::Transpose(F) + Q;
madcowswe 0:a0285293f6a6 62 }
madcowswe 0:a0285293f6a6 63
madcowswe 0:a0285293f6a6 64 void Kalman::update(int sonarid, float dist){
madcowswe 0:a0285293f6a6 65
madcowswe 0:a0285293f6a6 66 float rbx = X(1,1) - beaconx;
madcowswe 0:a0285293f6a6 67 float rby = X(2,1) - beacony;
madcowswe 0:a0285293f6a6 68
madcowswe 0:a0285293f6a6 69 float expecdist = sqrt(rbx*rbx + rby*rby);
madcowswe 0:a0285293f6a6 70 float Y = dist - expecdist;
madcowswe 0:a0285293f6a6 71
madcowswe 0:a0285293f6a6 72 float dhdx = rbx / expecdist;
madcowswe 0:a0285293f6a6 73 float dhdy = rby / expecdist;
madcowswe 0:a0285293f6a6 74 Matrix H(1,3);
madcowswe 0:a0285293f6a6 75 H << dhdx << dhdy << 0;
madcowswe 0:a0285293f6a6 76
madcowswe 0:a0285293f6a6 77 Matrix PH = P * MatrixMath::Transpose(H);
madcowswe 0:a0285293f6a6 78 float S = (H * PH)(1,1) + R;
madcowswe 0:a0285293f6a6 79 Matrix K = PH * (1/S);
madcowswe 0:a0285293f6a6 80
madcowswe 0:a0285293f6a6 81 //Updating state
madcowswe 0:a0285293f6a6 82 X += K*Y;
madcowswe 0:a0285293f6a6 83
madcowswe 0:a0285293f6a6 84 Matrix I3(3,3);
madcowswe 0:a0285293f6a6 85 I3 << 1 << 0 << 0
madcowswe 0:a0285293f6a6 86 << 0 << 1 << 0
madcowswe 0:a0285293f6a6 87 << 0 << 0 << 1;
madcowswe 0:a0285293f6a6 88 P = (I3 - K * H) * P;
madcowswe 0:a0285293f6a6 89
madcowswe 0:a0285293f6a6 90
madcowswe 0:a0285293f6a6 91
madcowswe 0:a0285293f6a6 92 }