File content as of revision 2:8aa491f77a0b:
//***************************************************************************************
//Kalman Filter implementation
//***************************************************************************************
#include "Kalman.h"
#include "rtos.h"
#include "RFSRF05.h"
#include "math.h"
#include "globals.h"
#include "motors.h"
#include "system.h"
#include "geometryfuncs.h"
#include <tvmet/Matrix.h>
#include <tvmet/Vector.h>
using namespace tvmet;
Kalman::Kalman(Motors &motorsin,
UI &uiin,
PinName Sonar_Trig,
PinName Sonar_Echo0,
PinName Sonar_Echo1,
PinName Sonar_Echo2,
PinName Sonar_Echo3,
PinName Sonar_Echo4,
PinName Sonar_Echo5,
PinName Sonar_SDI,
PinName Sonar_SDO,
PinName Sonar_SCK,
PinName Sonar_NCS,
PinName Sonar_NIRQ,
PinName IR_Tx,
PinName IR_Rx) :
ir(*this,IR_Tx,IR_Rx),
sonararray(Sonar_Trig,
Sonar_Echo0,
Sonar_Echo1,
Sonar_Echo2,
Sonar_Echo3,
Sonar_Echo4,
Sonar_Echo5,
Sonar_SDI,
Sonar_SDO,
Sonar_SCK,
Sonar_NCS,
Sonar_NIRQ),
motors(motorsin),
ui(uiin),
predictthread(predictloopwrapper, this, osPriorityNormal, 512),
predictticker( SIGTICKARGS(predictthread, 0x1) ),
// sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
// sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
updatethread(updateloopwrapper, this, osPriorityNormal, 512) {
Kalman_init = false;
//Intialising some arrays to zero
for (int kk = 0; kk < 3; kk ++) {
SonarMeasure_Offset[kk] = 0;
}
//Initialising other vars
//Initilising matrices
// X = x, y, theta;
X = 0.5, 0, 0;
P = 1, 0, 0,
0, 1, 0,
0, 0, 0.04;
//measurment variance R is provided by each sensor when calling runupdate
//attach callback
sonararray.callbackobj = (DummyCT*)this;
sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance, float variance)) &Kalman::runupdate;
predictticker.start(20);
// sonarticker.start(50);
}
//Note: this init function assumes that the robot faces east, theta=0, in the +x direction
void Kalman::KalmanInit() {
float SonarMeasuresx1000[3];
float IRMeasuresloc[3];
int beacon_cnt = 0;
// set initiating flag to false
Kalman_init = false;
// init the offset array
for (int k = 0; k < 3; k ++) {
SonarMeasure_Offset[k] = 0;
IRMeasures[k] = 0;
}
LPC_UART0->FCR = LPC_UART0->FCR | 0x06; // Flush the serial FIFO buffer / OR with FCR
//wating untill the IR has reved up and picked up some data
wait(1);
//temporaraly disable IR updates
ir.detachisr();
//IRturret.attach(NULL,Serial::RxIrq);
//lock the state throughout the computation, as we will override the state at the end
statelock.lock();
SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
IRMeasuresloc[0] = IRMeasures[0];
IRMeasuresloc[1] = IRMeasures[1];
IRMeasuresloc[2] = IRMeasures[2];
//printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
float d = beaconpos[2].y - beaconpos[1].y;
float i = beaconpos[0].y - beaconpos[1].y;
float j = beaconpos[0].x - beaconpos[1].x;
float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
float x_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
//Compute sonar offset
float Dist_Exp[3];
for (int k = 0; k < 3; k++) {
Dist_Exp[k] = sqrt((beaconpos[k].y - y_coor)*(beaconpos[k].y - y_coor)+(beaconpos[k].x - x_coor)*(beaconpos[k].x - x_coor));
SonarMeasure_Offset[k] = (SonarMeasuresx1000[k]-Dist_Exp[k])/1000.0f;
}
//Compute IR offset
ir.angleOffset = 0;
for (int i = 0; i < 3; i++) {
float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
// take average offset angle from valid readings
if (IRMeasuresloc[i] != 0) {
beacon_cnt ++;
// changed to current angle - estimated angle
float angle_temp = IRMeasuresloc[i] - angle_est;
angle_temp -= (floor(angle_temp/(2*PI)))*2*PI;
ir.angleOffset += angle_temp;
}
}
ir.angleOffset = ir.angleOffset/float(beacon_cnt);
//printf("\n\r");
//statelock already locked
ir.angleInit = true;
// set int flag to true
Kalman_init = true;
X(0) = x_coor/1000.0f;
X(1) = y_coor/1000.0f;
X(2) = 0;
statelock.unlock();
//printf("x: %0.4f, y: %0.4f, offset: %0.4f \n\r", x_coor, y_coor, angleOffset*180/PI);
//reattach the IR processing
ir.attachisr();
//IRturret.attach(&IR::vIRValueISR,Serial::RxIrq);
}
void Kalman::predictloop() {
float lastleft = 0;
float lastright = 0;
while (1) {
Thread::signal_wait(0x1);
OLED1 = !OLED1;
int leftenc = motors.getEncoder1();
int rightenc = motors.getEncoder2();
float dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f;
float dright = motors.encoderToDistance(rightenc-lastright)/1000.0f;
lastleft = leftenc;
lastright = rightenc;
//The below calculation are in body frame (where +x is forward)
float dxp, dyp,d,r;
float thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f);
if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
d = (dright + dleft)/2.0f;
dxp = d*cos(thetap/2.0f);
dyp = d*sin(thetap/2.0f);
} else { //calculate circle arc
//float r = (right + left) / (4.0f * PI * thetap);
r = (dright + dleft) / (2.0f*thetap);
dxp = abs(r)*sin(thetap);
dyp = r - r*cos(thetap);
}
statelock.lock();
//rotating to cartesian frame and updating state
X(0) += dxp * cos(X(2)) - dyp * sin(X(2));
X(1) += dxp * sin(X(2)) + dyp * cos(X(2));
X(2) = rectifyAng(X(2) + thetap);
//Linearising F around X
Matrix<float, 3, 3> F;
F = 1, 0, (dxp * -sin(X(2)) - dyp * cos(X(2))),
0, 1, (dxp * cos(X(2)) - dyp * sin(X(2))),
0, 0, 1;
//Generating forward and rotational variance
float varfwd = fwdvarperunit * (dright + dleft) / 2.0f;
float varang = varperang * thetap;
float varxydt = xyvarpertime * PREDICTPERIOD;
float varangdt = angvarpertime * PREDICTPERIOD;
//Rotating into cartesian frame
Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
Qsub = varfwd + varxydt, 0,
0, varxydt;
Qrot = Rotmatrix(X(2));
Qsubrot = Qrot * Qsub * trans(Qrot);
//Generate Q
Matrix<float, 3, 3> Q;//(Qsubrot);
Q = Qsubrot(0,0), Qsubrot(0,1), 0,
Qsubrot(1,0), Qsubrot(1,1), 0,
0, 0, varang + varangdt;
P = F * P * trans(F) + Q;
statelock.unlock();
//Thread::wait(PREDICTPERIOD);
//cout << "predict" << X << endl;
//cout << P << endl;
}
}
//void Kalman::sonarloop() {
// while (1) {
// Thread::signal_wait(0x1);
// sonararray.startRange();
// }
//}
void Kalman::runupdate(measurement_t type, float value, float variance) {
//printf("beacon %d dist %f\r\n", sonarid, dist);
//led2 = !led2;
measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
if (measured) {
measured->mtype = type;
measured->value = value;
measured->variance = variance;
osStatus putret = measureMQ.put(measured);
if (putret)
OLED4 = 1;
// printf("putting in MQ error code %#x\r\n", putret);
} else {
OLED4 = 1;
//printf("MQalloc returned NULL ptr\r\n");
}
}
void Kalman::updateloop() {
measurement_t type;
float value,variance,rbx,rby,expecdist,Y;
float dhdx,dhdy;
bool aborton2stddev = false;
Matrix<float, 1, 3> H;
float S;
Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
while (1) {
OLED2 = !OLED2;
osEvent evt = measureMQ.get();
if (evt.status == osEventMail) {
measurmentdata &measured = *(measurmentdata*)evt.value.p;
type = measured.mtype; //Note, may support more measurment types than sonar in the future!
value = measured.value;
variance = measured.variance;
// don't forget to free the memory
measureMQ.free(&measured);
if (type <= maxmeasure) {
if (type <= SONAR3) {
float dist = value / 1000.0f; //converting to m from mm
int sonarid = type;
aborton2stddev = false;
// Remove the offset if possible
if (Kalman_init)
dist = dist - SonarMeasure_Offset[sonarid];
statelock.lock();
//update the current sonar readings
SonarMeasures[sonarid] = dist;
rbx = X(0) - beaconpos[sonarid].x/1000.0f;
rby = X(1) - beaconpos[sonarid].y/1000.0f;
expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
Y = dist - expecdist;
dhdx = rbx / expecdist;
dhdy = rby / expecdist;
H = dhdx, dhdy, 0;
} else if (type <= IR3) {
aborton2stddev = false;
int IRidx = type-3;
statelock.lock();
IRMeasures[IRidx] = value;
rbx = X(0) - beaconpos[IRidx].x/1000.0f;
rby = X(1) - beaconpos[IRidx].y/1000.0f;
float expecang = atan2(-rby, -rbx) - X(2);
Y = rectifyAng(value - expecang);
float dstsq = rbx*rbx + rby*rby;
H = -rby/dstsq, rbx/dstsq, -1;
}
Matrix<float, 3, 1> PH (P * trans(H));
S = (H * PH)(0,0) + variance;
if (aborton2stddev && Y*Y > 4 * S) {
statelock.unlock();
continue;
}
Matrix<float, 3, 1> K (PH * (1/S));
//Updating state
X += col(K, 0) * Y;
X(2) = rectifyAng(X(2));
P = (I3 - K * H) * P;
statelock.unlock();
}
} else {
OLED4 = 1;
//printf("ERROR: in updateloop, code %#x", evt);
}
}
}