Ryan Savitski
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
-