Shuto Naruse
Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Diff: Eurobot_shared/Kalman/Kalman.cpp
- Revision:
- 25:143b19c1fb05
- Parent:
- 24:7a3906c2f5d5
- Child:
- 26:0995f61cb7b8
--- a/Eurobot_shared/Kalman/Kalman.cpp Fri May 04 05:23:45 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,467 +0,0 @@
-//***************************************************************************************
-//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) :
- ir(*this),
- 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) {
-
- //Initilising offsets
- InitLock.lock();
- IR_Offset = 0;
- Sonar_Offset = 0;
- InitLock.unlock();
-
-
- //Initilising matrices
-
- // X = x, y, theta;
- if (Colour)
- X = 0.5, 0, 0;
- else
- X = 2.5, 0, PI;
-
- 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() {
- motors.stop();
- float SonarMeasuresx1000[3];
- float IRMeasuresloc[3];
- int beacon_cnt = 0;
-
-
-// doesn't work since they break the ISR
- /*
- #ifdef ROBOT_PRIMARY
- LPC_UART3->FCR = LPC_UART3->FCR | 0x06; // Flush the serial FIFO buffer / OR with FCR
- #else
- LPC_UART1->FCR = LPC_UART1->FCR | 0x06; // Flush the serial FIFO buffer / OR with FCR
- #endif
- */
- // zeros the measurements
- for (int i = 0; i < 3; i++) {
- SonarMeasures[i] = 0;
- IRMeasures[i] = 0;
- }
-
- InitLock.lock();
- //zeros offsets
- IR_Offset = 0;
- Sonar_Offset = 0;
- InitLock.unlock();
-
- // attaches ir interrup
- ir.attachisr();
-
- //wating untill the IR has reved up and picked up some valid data
- //Thread::wait(1000);
- wait(2);
-
- //temporaraly disable IR updates
- ir.detachisr();
-
- //lock the state throughout the computation, as we will override the state at the end
- InitLock.lock();
- 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 origin_x = beaconpos[1].x;
- float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
- float x_coor = origin_x + (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
-
- //debug for trilateration
- printf("Cal at x: %0.4f, y: %0.4f \r\n",x_coor,y_coor );
-
- float Dist_Exp[3];
- for (int i = 0; i < 3; i++) {
- //Compute sonar offset
- Dist_Exp[i] = hypot(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
- Sonar_Offset += (SonarMeasuresx1000[i]-Dist_Exp[i])/3000.0f;
-
- //Compute IR offset
- float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
- if (!Colour)
- angle_est -= PI;
- //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
- // 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_Offset += angle_temp;
- }
- }
- IR_Offset /= float(beacon_cnt);
-
- //debug
- printf("Offsets IR: %0.4f, Sonar: %0.4f \r\n",IR_Offset*180/PI,Sonar_Offset*1000 );
-
- //statelock already locked
- X(0) = x_coor/1000.0f;
- X(1) = y_coor/1000.0f;
-
- if (Colour)
- X(2) = 0;
- else
- X(2) = PI;
-
- // unlocks mutexes
- InitLock.unlock();
- statelock.unlock();
-
-
- //reattach the IR processing
- ir.attachisr();
-}
-
-
-void Kalman::predictloop() {
-
- OLED4 = !ui.regid(0, 3);
- OLED4 = !ui.regid(1, 4);
-
- 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();
-
- float tempX2 = X(2);
- //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
- float avgX2 = (X(2) + tempX2)/2.0f;
- Matrix<float, 3, 3> F;
- F = 1, 0, (dxp * -sin(avgX2) - dyp * cos(avgX2)),
- 0, 1, (dxp * cos(avgX2) - dyp * sin(avgX2)),
- 0, 0, 1;
-
- //Generating forward and rotational variance
- float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f;
- float varang = varperang * abs(thetap);
- float varxydt = xyvarpertime * PREDICTPERIOD/1000.0f;
- float varangdt = angvarpertime * PREDICTPERIOD/1000.0f;
-
- //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;
-
- //Update UI
- float statecpy[] = {X(0), X(1), X(2)};
- ui.updateval(0, statecpy, 3);
-
- float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
- ui.updateval(1, Pcpy, 4);
-
- statelock.unlock();
- }
-}
-
-//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() {
-
- //sonar Y chanels
- ui.regid(2, 1);
- ui.regid(3, 1);
- ui.regid(4, 1);
-
- //IR Y chanels
- ui.regid(5, 1);
- ui.regid(6, 1);
- ui.regid(7, 1);
-
- 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) {
-
- InitLock.lock();
- float dist = value / 1000.0f - Sonar_Offset; //converting to m from mm,subtract the offset
- InitLock.unlock();
-
- int sonarid = type;
- aborton2stddev = true;
-
- 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;
-
- //send to ui
- ui.updateval(sonarid+2, Y);
-
- dhdx = rbx / expecdist;
- dhdy = rby / expecdist;
-
- H = dhdx, dhdy, 0;
-
- } else if (type <= IR3) {
-
- aborton2stddev = false;
- int IRidx = type-3;
-
- // subtract the IR offset
- InitLock.lock();
- value -= IR_Offset;
- InitLock.unlock();
-
- 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);
-
- //send to ui
- ui.updateval(IRidx + 5, Y);
-
- 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);
- }
-
- }
-
-}
-
-// reset kalman states
-void Kalman::KalmanReset() {
- float SonarMeasuresx1000[3];
- statelock.lock();
- SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
- SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
- SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
- //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 origin_x = beaconpos[1].x;
- float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
- float x_coor = origin_x +(SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
-
- //statelock already locked
- X(0) = x_coor/1000.0f;
- X(1) = y_coor/1000.0f;
-
-
-
-/* if (Colour){
- X(0) = 0.2;
- X(1) = 0.2;
- //X(2) = 0;
- }
- else {
- X(0) = 2.8;
- X(1) = 0.2;
- //X(2) = PI;
- }
- */
- P = 0.05, 0, 0,
- 0, 0.05, 0,
- 0, 0, 0.04;
-
- // unlocks mutexes
- statelock.unlock();
-
-}
\ No newline at end of file