2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/Kalman/Kalman.cpp
- Revision:
- 20:70d651156779
- Parent:
- 19:4b993a9a156e
- Child:
- 21:167dacfe0b14
diff -r 4b993a9a156e -r 70d651156779 Processes/Kalman/Kalman.cpp --- a/Processes/Kalman/Kalman.cpp Sun Apr 07 19:26:07 2013 +0000 +++ b/Processes/Kalman/Kalman.cpp Tue Apr 09 15:33:36 2013 +0000 @@ -5,9 +5,10 @@ #include "rtos.h" #include "math.h" #include "supportfuncs.h" +#include "Encoder.h" //#include "globals.h" -#include <tvmet/Matrix.h> +#include "tvmet/Matrix.h" using namespace tvmet; @@ -15,6 +16,11 @@ namespace Kalman { +Ticker predictticker; + +DigitalOut OLED4(LED4); +DigitalOut OLED1(LED1); + //State variables Matrix<float, 3, 1> X; Matrix<float, 3, 3> P; @@ -23,7 +29,7 @@ float RawReadings[maxmeasure+1]; float IRpahseOffset; -bool Kalman_init = 0; +bool Kalman_inited = 0; struct measurmentdata { measurement_t mtype; @@ -33,29 +39,41 @@ Mail <measurmentdata, 16> measureMQ; +Thread* predict_thread_ptr = NULL; //Note: this init function assumes that the robot faces east, theta=0, in the +x direction void KalmanInit() { + printf("kalmaninit \r\n"); + + //WARNING: HARDCODED! + //solve for our position (assume perfect bias) - const float d = beaconpos[0].y - beaconpos[1].y; - const float i = beaconpos[0].y - beaconpos[2].y; - const float j = beaconpos[0].x - beaconpos[2].x; - float r1 = RawReadings[SONAR0]; + const float d = beaconpos[2].y - beaconpos[1].y; + const float i = beaconpos[2].y - beaconpos[0].y; + const float j = beaconpos[2].x - beaconpos[0].x; + float r1 = RawReadings[SONAR2]; float r2 = RawReadings[SONAR1]; - float r3 = RawReadings[SONAR2]; + float r3 = RawReadings[SONAR0]; + + printf("ranges: 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", r1, r2, r3); float y_coor = (r1*r1-r2*r2+d*d)/(2*d); float x_coor = (r1*r1-r3*r3+i*i+j*j)/(2*j) - (i*y_coor)/j; - + + //coordinate system hack (for now) + x_coor = beaconpos[2].x - x_coor; + y_coor = beaconpos[2].y - y_coor; + + printf("solved pos from sonar: %f, %f \r\n", x_coor, y_coor); + //IR - float IRMeasuresloc[3]; IRMeasuresloc[0] = RawReadings[IR0]; IRMeasuresloc[1] = RawReadings[IR1]; IRMeasuresloc[2] = RawReadings[IR2]; - //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI); + printf("IR 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI); float IR_Offsets[3]; float fromb0offset = 0; @@ -66,10 +84,10 @@ //printf("Angle %d : %f \n\r",i,angle_est*180/PI ); IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est); - + fromb0offset += constrainAngle(IR_Offsets[i] - IR_Offsets[0]); } - + IRpahseOffset = constrainAngle(IR_Offsets[0] + fromb0offset/3); //debug @@ -80,14 +98,24 @@ X(1,0) = y_coor; X(2,0) = 0; statelock.unlock(); + + Kalman_inited = 1; } -/* -void Kalman::predictloop(void* dummy) + +State getState(){ + statelock.lock(); + State state = {X(0,0), X(1,0), X(2,0)}; + statelock.unlock(); + return state; +} + + +void predictloop(void const *dummy) { - OLED4 = !ui.regid(0, 3); - OLED4 = !ui.regid(1, 4); + //OLED4 = !ui.regid(0, 3); + //OLED4 = !ui.regid(1, 4); float lastleft = 0; float lastright = 0; @@ -96,11 +124,11 @@ Thread::signal_wait(0x1); OLED1 = !OLED1; - int leftenc = encoders.getEncoder1(); - int rightenc = encoders.getEncoder2(); + float leftenc = left_encoder.getTicks() * ENCODER_M_PER_TICK; + float rightenc = right_encoder.getTicks() * ENCODER_M_PER_TICK; - float dleft = encoders.encoderToDistance(leftenc-lastleft)/1000.0f; - float dright = encoders.encoderToDistance(rightenc-lastright)/1000.0f; + float dleft = leftenc-lastleft; + float dright = rightenc-lastright; lastleft = leftenc; lastright = rightenc; @@ -108,8 +136,8 @@ //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 + float thetap = (dright - dleft) / ENCODER_WHEELBASE; + if (abs(thetap) < 0.01f) { //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); @@ -117,20 +145,20 @@ } else { //calculate circle arc //float r = (right + left) / (4.0f * PI * thetap); r = (dright + dleft) / (2.0f*thetap); - dxp = abs(r)*sin(thetap); + dxp = r*sin(thetap); dyp = r - r*cos(thetap); } statelock.lock(); - float tempX2 = X(2); + float tempX2 = X(2,0); //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); + X(0,0) += dxp * cos(X(2,0)) - dyp * sin(X(2,0)); + X(1,0) += dxp * sin(X(2,0)) + dyp * cos(X(2,0)); + X(2,0) = constrainAngle(X(2,0) + thetap); //Linearising F around X - float avgX2 = (X(2) + tempX2)/2.0f; + float avgX2 = (X(2,0) + 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)), @@ -139,15 +167,15 @@ //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; + float varxydt = xyvarpertime * KALMAN_PREDICT_PERIOD; + float varangdt = angvarpertime * KALMAN_PREDICT_PERIOD; //Rotating into cartesian frame Matrix<float, 2, 2> Qsub,Qsubrot,Qrot; Qsub = varfwd + varxydt, 0, 0, varxydt; - Qrot = Rotmatrix(X(2)); + Qrot = Rotmatrix(X(2,0)); Qsubrot = Qrot * Qsub * trans(Qrot); @@ -159,24 +187,42 @@ P = F * P * trans(F) + Q; + //printf("x: %f, y: %f, t: %f\r\n", X(0,0), X(1,0), X(2,0)); //Update UI - float statecpy[] = {X(0), X(1), X(2)}; - ui.updateval(0, statecpy, 3); + //float statecpy[] = {X(0,0), X(1,0), X(2,0)}; + //ui.updateval(0, statecpy, 3); - float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)}; - ui.updateval(1, Pcpy, 4); + //float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)}; + //ui.updateval(1, Pcpy, 4); statelock.unlock(); } } -void Kalman::runupdate(measurement_t type, float value, float variance) + +void predict_event_setter(){ + if(predict_thread_ptr) + predict_thread_ptr->signal_set(0x1); + else + OLED4 = 1; +} + +void start_predict_ticker(Thread* predict_thread_ptr_in){ + predict_thread_ptr = predict_thread_ptr_in; + predictticker.attach(predict_event_setter, KALMAN_PREDICT_PERIOD); +} + +void runupdate(measurement_t type, float value, float variance) { - if (!Kalman_init) + if (!Kalman_inited) { RawReadings[type] = value; - else { + } else { - RawReadings[type] = value - SensorOffsets[type]; + if (type >= IR0 && type <= IR2) + RawReadings[type] = value - IRpahseOffset; + else + RawReadings[type] = value; + measurmentdata* measured = (measurmentdata*)measureMQ.alloc(); if (measured) { @@ -185,19 +231,20 @@ measured->variance = variance; osStatus putret = measureMQ.put(measured); - if (putret) - OLED4 = 1; + //if (putret) + //OLED4 = 1; // printf("putting in MQ error code %#x\r\n", putret); } else { - OLED4 = 1; + //OLED4 = 1; //printf("MQalloc returned NULL ptr\r\n"); } - + } + } - -void Kalman::updateloop(void* dummy) +/* +void Kalman::updateloop(void const *dummy) { //sonar Y chanels @@ -320,37 +367,6 @@ } -// 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; - - P = 0.05, 0, 0, - 0, 0.05, 0, - 0, 0, 0.04; - - // unlocks mutexes - statelock.unlock(); - -} - */ } //Kalman Namespace -