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;
    
    

}