Kalman filter for Eurobot

## Kalman.cpp

Committer:
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;

}
```