Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/Kalman/Kalman.cpp@17:6263e90bf3ba, 2013-04-07 (annotated)

Committer:
madcowswe
Date:
Sun Apr 07 17:51:59 2013 +0000
Revision:
17:6263e90bf3ba
Parent:
16:52250d8d8fce
Child:
19:4b993a9a156e
Still fixing init in kalman

Who changed what in which revision?

UserRevisionLine numberNew contents of line
madcowswe 16:52250d8d8fce 1 //***************************************************************************************
madcowswe 16:52250d8d8fce 2 //Kalman Filter implementation
madcowswe 16:52250d8d8fce 3 //***************************************************************************************
madcowswe 16:52250d8d8fce 4 #include "Kalman.h"
madcowswe 16:52250d8d8fce 5 #include "rtos.h"
madcowswe 16:52250d8d8fce 6 #include "math.h"
madcowswe 16:52250d8d8fce 7 #include "supportfuncs.h"
madcowswe 16:52250d8d8fce 8 //#include "globals.h"
madcowswe 16:52250d8d8fce 9
madcowswe 16:52250d8d8fce 10 #include <tvmet/Matrix.h>
madcowswe 16:52250d8d8fce 11 using namespace tvmet;
madcowswe 16:52250d8d8fce 12
madcowswe 16:52250d8d8fce 13
madcowswe 16:52250d8d8fce 14
madcowswe 16:52250d8d8fce 15 namespace Kalman
madcowswe 16:52250d8d8fce 16 {
madcowswe 16:52250d8d8fce 17
madcowswe 16:52250d8d8fce 18 //State variables
madcowswe 16:52250d8d8fce 19 Vector<float, 3> X;
madcowswe 16:52250d8d8fce 20 Matrix<float, 3, 3> P;
madcowswe 16:52250d8d8fce 21 Mutex statelock;
madcowswe 16:52250d8d8fce 22
madcowswe 16:52250d8d8fce 23 float RawReadings[maxmeasure+1];
madcowswe 16:52250d8d8fce 24 float SensorOffsets[maxmeasure+1] = {0};
madcowswe 16:52250d8d8fce 25
madcowswe 16:52250d8d8fce 26 bool Kalman_init = 0;
madcowswe 16:52250d8d8fce 27
madcowswe 16:52250d8d8fce 28 struct measurmentdata {
madcowswe 16:52250d8d8fce 29 measurement_t mtype;
madcowswe 16:52250d8d8fce 30 float value;
madcowswe 16:52250d8d8fce 31 float variance;
madcowswe 16:52250d8d8fce 32 }
madcowswe 16:52250d8d8fce 33
madcowswe 16:52250d8d8fce 34 Mail <measurmentdata, 16> measureMQ;
madcowswe 16:52250d8d8fce 35
madcowswe 16:52250d8d8fce 36
madcowswe 16:52250d8d8fce 37
madcowswe 16:52250d8d8fce 38 //Note: this init function assumes that the robot faces east, theta=0, in the +x direction
madcowswe 16:52250d8d8fce 39 void KalmanInit()
madcowswe 16:52250d8d8fce 40 {
madcowswe 17:6263e90bf3ba 41
madcowswe 16:52250d8d8fce 42 //Solving for sonar bias is done by entering the following into wolfram alpha
madcowswe 16:52250d8d8fce 43 //(a-f)^2 = x^2 + y^2; (b-f)^2 = (x-3)^2 + y^2; (c-f)^2 = (x-1.5)^2+(y-2)^2: solve for x,y,f
madcowswe 16:52250d8d8fce 44 //where a, b, c are the measured distances, and f is the bias
madcowswe 17:6263e90bf3ba 45
madcowswe 16:52250d8d8fce 46 SensorOffsets[SONAR0] = sonartimebias;
madcowswe 16:52250d8d8fce 47 SensorOffsets[SONAR1] = sonartimebias;
madcowswe 16:52250d8d8fce 48 SensorOffsets[SONAR2] = sonartimebias;
madcowswe 17:6263e90bf3ba 49
madcowswe 16:52250d8d8fce 50 //solve for our position (assume perfect bias)
madcowswe 17:6263e90bf3ba 51 const float d = beaconpos[0].y - beaconpos[1].y;
madcowswe 17:6263e90bf3ba 52 const float i = beaconpos[0].y - beaconpos[2].y;
madcowswe 17:6263e90bf3ba 53 const float j = beaconpos[0].x - beaconpos[2].x;
madcowswe 17:6263e90bf3ba 54
madcowswe 17:6263e90bf3ba 55 float y_coor = (r1*r1-r2*r2+d*d)/2d;
madcowswe 17:6263e90bf3ba 56 float x_coor = (r1*r1-r3*r3+i*i+j*j)/(2*j) - (i*y_coor)/j;
madcowswe 17:6263e90bf3ba 57
madcowswe 16:52250d8d8fce 58 //IR
madcowswe 16:52250d8d8fce 59
madcowswe 16:52250d8d8fce 60 float IRMeasuresloc[3];
madcowswe 16:52250d8d8fce 61 IRMeasuresloc[0] = RawReadings[IR0];
madcowswe 16:52250d8d8fce 62 IRMeasuresloc[1] = RawReadings[IR1];
madcowswe 16:52250d8d8fce 63 IRMeasuresloc[2] = RawReadings[IR2];
madcowswe 16:52250d8d8fce 64 //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
madcowswe 16:52250d8d8fce 65
madcowswe 17:6263e90bf3ba 66 float IR_Offsets[3];
madcowswe 17:6263e90bf3ba 67 float fromb0offset = 0;
madcowswe 16:52250d8d8fce 68 for (int i = 0; i < 3; i++) {
madcowswe 16:52250d8d8fce 69
madcowswe 16:52250d8d8fce 70 //Compute IR offset
madcowswe 16:52250d8d8fce 71 float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
madcowswe 17:6263e90bf3ba 72
madcowswe 16:52250d8d8fce 73 //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
madcowswe 17:6263e90bf3ba 74 IR_Offset[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
madcowswe 16:52250d8d8fce 75
madcowswe 17:6263e90bf3ba 76 fromb0offset += IR_Offsets[i] - IR_Offset[0];
madcowswe 16:52250d8d8fce 77 }
madcowswe 17:6263e90bf3ba 78
madcowswe 16:52250d8d8fce 79
madcowswe 16:52250d8d8fce 80 //debug
madcowswe 16:52250d8d8fce 81 printf("Offsets IR: %0.4f, Sonar: %0.4f \r\n",IR_Offset*180/PI,Sonar_Offset*1000 );
madcowswe 16:52250d8d8fce 82
madcowswe 16:52250d8d8fce 83
madcowswe 16:52250d8d8fce 84 statelock.lock();
madcowswe 16:52250d8d8fce 85 X(0) = x_coor/1000.0f;
madcowswe 16:52250d8d8fce 86 X(1) = y_coor/1000.0f;
madcowswe 16:52250d8d8fce 87
madcowswe 16:52250d8d8fce 88 if (Colour)
madcowswe 16:52250d8d8fce 89 X(2) = 0;
madcowswe 16:52250d8d8fce 90 else
madcowswe 16:52250d8d8fce 91 X(2) = PI;
madcowswe 16:52250d8d8fce 92 statelock.unlock();
madcowswe 16:52250d8d8fce 93
madcowswe 16:52250d8d8fce 94
madcowswe 16:52250d8d8fce 95 //reattach the IR processing
madcowswe 16:52250d8d8fce 96 ir.attachisr();
madcowswe 16:52250d8d8fce 97 }
madcowswe 16:52250d8d8fce 98
madcowswe 16:52250d8d8fce 99
madcowswe 16:52250d8d8fce 100 void Kalman::predictloop(void* dummy)
madcowswe 16:52250d8d8fce 101 {
madcowswe 16:52250d8d8fce 102
madcowswe 16:52250d8d8fce 103 OLED4 = !ui.regid(0, 3);
madcowswe 16:52250d8d8fce 104 OLED4 = !ui.regid(1, 4);
madcowswe 16:52250d8d8fce 105
madcowswe 16:52250d8d8fce 106 float lastleft = 0;
madcowswe 16:52250d8d8fce 107 float lastright = 0;
madcowswe 16:52250d8d8fce 108
madcowswe 16:52250d8d8fce 109 while (1) {
madcowswe 16:52250d8d8fce 110 Thread::signal_wait(0x1);
madcowswe 16:52250d8d8fce 111 OLED1 = !OLED1;
madcowswe 16:52250d8d8fce 112
madcowswe 16:52250d8d8fce 113 int leftenc = encoders.getEncoder1();
madcowswe 16:52250d8d8fce 114 int rightenc = encoders.getEncoder2();
madcowswe 16:52250d8d8fce 115
madcowswe 16:52250d8d8fce 116 float dleft = encoders.encoderToDistance(leftenc-lastleft)/1000.0f;
madcowswe 16:52250d8d8fce 117 float dright = encoders.encoderToDistance(rightenc-lastright)/1000.0f;
madcowswe 16:52250d8d8fce 118
madcowswe 16:52250d8d8fce 119 lastleft = leftenc;
madcowswe 16:52250d8d8fce 120 lastright = rightenc;
madcowswe 16:52250d8d8fce 121
madcowswe 16:52250d8d8fce 122
madcowswe 16:52250d8d8fce 123 //The below calculation are in body frame (where +x is forward)
madcowswe 16:52250d8d8fce 124 float dxp, dyp,d,r;
madcowswe 16:52250d8d8fce 125 float thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f);
madcowswe 16:52250d8d8fce 126 if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
madcowswe 16:52250d8d8fce 127 d = (dright + dleft)/2.0f;
madcowswe 16:52250d8d8fce 128 dxp = d*cos(thetap/2.0f);
madcowswe 16:52250d8d8fce 129 dyp = d*sin(thetap/2.0f);
madcowswe 16:52250d8d8fce 130
madcowswe 16:52250d8d8fce 131 } else { //calculate circle arc
madcowswe 16:52250d8d8fce 132 //float r = (right + left) / (4.0f * PI * thetap);
madcowswe 16:52250d8d8fce 133 r = (dright + dleft) / (2.0f*thetap);
madcowswe 16:52250d8d8fce 134 dxp = abs(r)*sin(thetap);
madcowswe 16:52250d8d8fce 135 dyp = r - r*cos(thetap);
madcowswe 16:52250d8d8fce 136 }
madcowswe 16:52250d8d8fce 137
madcowswe 16:52250d8d8fce 138 statelock.lock();
madcowswe 16:52250d8d8fce 139
madcowswe 16:52250d8d8fce 140 float tempX2 = X(2);
madcowswe 16:52250d8d8fce 141 //rotating to cartesian frame and updating state
madcowswe 16:52250d8d8fce 142 X(0) += dxp * cos(X(2)) - dyp * sin(X(2));
madcowswe 16:52250d8d8fce 143 X(1) += dxp * sin(X(2)) + dyp * cos(X(2));
madcowswe 16:52250d8d8fce 144 X(2) = rectifyAng(X(2) + thetap);
madcowswe 16:52250d8d8fce 145
madcowswe 16:52250d8d8fce 146 //Linearising F around X
madcowswe 16:52250d8d8fce 147 float avgX2 = (X(2) + tempX2)/2.0f;
madcowswe 16:52250d8d8fce 148 Matrix<float, 3, 3> F;
madcowswe 16:52250d8d8fce 149 F = 1, 0, (dxp * -sin(avgX2) - dyp * cos(avgX2)),
madcowswe 16:52250d8d8fce 150 0, 1, (dxp * cos(avgX2) - dyp * sin(avgX2)),
madcowswe 16:52250d8d8fce 151 0, 0, 1;
madcowswe 16:52250d8d8fce 152
madcowswe 16:52250d8d8fce 153 //Generating forward and rotational variance
madcowswe 16:52250d8d8fce 154 float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f;
madcowswe 16:52250d8d8fce 155 float varang = varperang * abs(thetap);
madcowswe 16:52250d8d8fce 156 float varxydt = xyvarpertime * PREDICTPERIOD/1000.0f;
madcowswe 16:52250d8d8fce 157 float varangdt = angvarpertime * PREDICTPERIOD/1000.0f;
madcowswe 16:52250d8d8fce 158
madcowswe 16:52250d8d8fce 159 //Rotating into cartesian frame
madcowswe 16:52250d8d8fce 160 Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
madcowswe 16:52250d8d8fce 161 Qsub = varfwd + varxydt, 0,
madcowswe 16:52250d8d8fce 162 0, varxydt;
madcowswe 16:52250d8d8fce 163
madcowswe 16:52250d8d8fce 164 Qrot = Rotmatrix(X(2));
madcowswe 16:52250d8d8fce 165
madcowswe 16:52250d8d8fce 166 Qsubrot = Qrot * Qsub * trans(Qrot);
madcowswe 16:52250d8d8fce 167
madcowswe 16:52250d8d8fce 168 //Generate Q
madcowswe 16:52250d8d8fce 169 Matrix<float, 3, 3> Q;//(Qsubrot);
madcowswe 16:52250d8d8fce 170 Q = Qsubrot(0,0), Qsubrot(0,1), 0,
madcowswe 16:52250d8d8fce 171 Qsubrot(1,0), Qsubrot(1,1), 0,
madcowswe 16:52250d8d8fce 172 0, 0, varang + varangdt;
madcowswe 16:52250d8d8fce 173
madcowswe 16:52250d8d8fce 174 P = F * P * trans(F) + Q;
madcowswe 16:52250d8d8fce 175
madcowswe 16:52250d8d8fce 176 //Update UI
madcowswe 16:52250d8d8fce 177 float statecpy[] = {X(0), X(1), X(2)};
madcowswe 16:52250d8d8fce 178 ui.updateval(0, statecpy, 3);
madcowswe 16:52250d8d8fce 179
madcowswe 16:52250d8d8fce 180 float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
madcowswe 16:52250d8d8fce 181 ui.updateval(1, Pcpy, 4);
madcowswe 16:52250d8d8fce 182
madcowswe 16:52250d8d8fce 183 statelock.unlock();
madcowswe 16:52250d8d8fce 184 }
madcowswe 16:52250d8d8fce 185 }
madcowswe 16:52250d8d8fce 186
madcowswe 16:52250d8d8fce 187 void Kalman::runupdate(measurement_t type, float value, float variance)
madcowswe 16:52250d8d8fce 188 {
madcowswe 16:52250d8d8fce 189 if (!Kalman_init)
madcowswe 16:52250d8d8fce 190 RawReadings[type] = value;
madcowswe 16:52250d8d8fce 191 else {
madcowswe 17:6263e90bf3ba 192
madcowswe 16:52250d8d8fce 193 RawReadings[type] = value - SensorOffsets[type];
madcowswe 17:6263e90bf3ba 194
madcowswe 16:52250d8d8fce 195 measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
madcowswe 16:52250d8d8fce 196 if (measured) {
madcowswe 16:52250d8d8fce 197 measured->mtype = type;
madcowswe 16:52250d8d8fce 198 measured->value = value;
madcowswe 16:52250d8d8fce 199 measured->variance = variance;
madcowswe 16:52250d8d8fce 200
madcowswe 16:52250d8d8fce 201 osStatus putret = measureMQ.put(measured);
madcowswe 16:52250d8d8fce 202 if (putret)
madcowswe 16:52250d8d8fce 203 OLED4 = 1;
madcowswe 16:52250d8d8fce 204 // printf("putting in MQ error code %#x\r\n", putret);
madcowswe 16:52250d8d8fce 205 } else {
madcowswe 16:52250d8d8fce 206 OLED4 = 1;
madcowswe 16:52250d8d8fce 207 //printf("MQalloc returned NULL ptr\r\n");
madcowswe 16:52250d8d8fce 208 }
madcowswe 16:52250d8d8fce 209
madcowswe 16:52250d8d8fce 210 }
madcowswe 16:52250d8d8fce 211
madcowswe 16:52250d8d8fce 212 }
madcowswe 16:52250d8d8fce 213
madcowswe 16:52250d8d8fce 214 void Kalman::updateloop(void* dummy)
madcowswe 16:52250d8d8fce 215 {
madcowswe 16:52250d8d8fce 216
madcowswe 16:52250d8d8fce 217 //sonar Y chanels
madcowswe 16:52250d8d8fce 218 ui.regid(2, 1);
madcowswe 16:52250d8d8fce 219 ui.regid(3, 1);
madcowswe 16:52250d8d8fce 220 ui.regid(4, 1);
madcowswe 16:52250d8d8fce 221
madcowswe 16:52250d8d8fce 222 //IR Y chanels
madcowswe 16:52250d8d8fce 223 ui.regid(5, 1);
madcowswe 16:52250d8d8fce 224 ui.regid(6, 1);
madcowswe 16:52250d8d8fce 225 ui.regid(7, 1);
madcowswe 16:52250d8d8fce 226
madcowswe 16:52250d8d8fce 227 measurement_t type;
madcowswe 16:52250d8d8fce 228 float value,variance,rbx,rby,expecdist,Y;
madcowswe 16:52250d8d8fce 229 float dhdx,dhdy;
madcowswe 16:52250d8d8fce 230 bool aborton2stddev = false;
madcowswe 16:52250d8d8fce 231
madcowswe 16:52250d8d8fce 232 Matrix<float, 1, 3> H;
madcowswe 16:52250d8d8fce 233
madcowswe 16:52250d8d8fce 234 float S;
madcowswe 16:52250d8d8fce 235 Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
madcowswe 16:52250d8d8fce 236
madcowswe 16:52250d8d8fce 237
madcowswe 16:52250d8d8fce 238 while (1) {
madcowswe 16:52250d8d8fce 239 OLED2 = !OLED2;
madcowswe 16:52250d8d8fce 240
madcowswe 16:52250d8d8fce 241 osEvent evt = measureMQ.get();
madcowswe 16:52250d8d8fce 242
madcowswe 16:52250d8d8fce 243 if (evt.status == osEventMail) {
madcowswe 16:52250d8d8fce 244
madcowswe 16:52250d8d8fce 245 measurmentdata &measured = *(measurmentdata*)evt.value.p;
madcowswe 16:52250d8d8fce 246 type = measured.mtype; //Note, may support more measurment types than sonar in the future!
madcowswe 16:52250d8d8fce 247 value = measured.value;
madcowswe 16:52250d8d8fce 248 variance = measured.variance;
madcowswe 16:52250d8d8fce 249
madcowswe 16:52250d8d8fce 250 // don't forget to free the memory
madcowswe 16:52250d8d8fce 251 measureMQ.free(&measured);
madcowswe 16:52250d8d8fce 252
madcowswe 16:52250d8d8fce 253 if (type <= maxmeasure) {
madcowswe 16:52250d8d8fce 254
madcowswe 16:52250d8d8fce 255 if (type <= SONAR3) {
madcowswe 16:52250d8d8fce 256
madcowswe 16:52250d8d8fce 257 InitLock.lock();
madcowswe 16:52250d8d8fce 258 float dist = value / 1000.0f - Sonar_Offset; //converting to m from mm,subtract the offset
madcowswe 16:52250d8d8fce 259 InitLock.unlock();
madcowswe 16:52250d8d8fce 260
madcowswe 16:52250d8d8fce 261 int sonarid = type;
madcowswe 16:52250d8d8fce 262 aborton2stddev = true;
madcowswe 16:52250d8d8fce 263
madcowswe 16:52250d8d8fce 264 statelock.lock();
madcowswe 16:52250d8d8fce 265 //update the current sonar readings
madcowswe 16:52250d8d8fce 266 SonarMeasures[sonarid] = dist;
madcowswe 16:52250d8d8fce 267
madcowswe 16:52250d8d8fce 268 rbx = X(0) - beaconpos[sonarid].x/1000.0f;
madcowswe 16:52250d8d8fce 269 rby = X(1) - beaconpos[sonarid].y/1000.0f;
madcowswe 16:52250d8d8fce 270
madcowswe 16:52250d8d8fce 271 expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
madcowswe 16:52250d8d8fce 272 Y = dist - expecdist;
madcowswe 16:52250d8d8fce 273
madcowswe 16:52250d8d8fce 274 //send to ui
madcowswe 16:52250d8d8fce 275 ui.updateval(sonarid+2, Y);
madcowswe 16:52250d8d8fce 276
madcowswe 16:52250d8d8fce 277 dhdx = rbx / expecdist;
madcowswe 16:52250d8d8fce 278 dhdy = rby / expecdist;
madcowswe 16:52250d8d8fce 279
madcowswe 16:52250d8d8fce 280 H = dhdx, dhdy, 0;
madcowswe 16:52250d8d8fce 281
madcowswe 16:52250d8d8fce 282 } else if (type <= IR3) {
madcowswe 16:52250d8d8fce 283
madcowswe 16:52250d8d8fce 284 aborton2stddev = false;
madcowswe 16:52250d8d8fce 285 int IRidx = type-3;
madcowswe 16:52250d8d8fce 286
madcowswe 16:52250d8d8fce 287 // subtract the IR offset
madcowswe 16:52250d8d8fce 288 InitLock.lock();
madcowswe 16:52250d8d8fce 289 value -= IR_Offset;
madcowswe 16:52250d8d8fce 290 InitLock.unlock();
madcowswe 16:52250d8d8fce 291
madcowswe 16:52250d8d8fce 292 statelock.lock();
madcowswe 16:52250d8d8fce 293 IRMeasures[IRidx] = value;
madcowswe 16:52250d8d8fce 294
madcowswe 16:52250d8d8fce 295 rbx = X(0) - beaconpos[IRidx].x/1000.0f;
madcowswe 16:52250d8d8fce 296 rby = X(1) - beaconpos[IRidx].y/1000.0f;
madcowswe 16:52250d8d8fce 297
madcowswe 16:52250d8d8fce 298 float expecang = atan2(-rby, -rbx) - X(2);
madcowswe 16:52250d8d8fce 299 Y = rectifyAng(value - expecang);
madcowswe 16:52250d8d8fce 300
madcowswe 16:52250d8d8fce 301 //send to ui
madcowswe 16:52250d8d8fce 302 ui.updateval(IRidx + 5, Y);
madcowswe 16:52250d8d8fce 303
madcowswe 16:52250d8d8fce 304 float dstsq = rbx*rbx + rby*rby;
madcowswe 16:52250d8d8fce 305 H = -rby/dstsq, rbx/dstsq, -1;
madcowswe 16:52250d8d8fce 306 }
madcowswe 16:52250d8d8fce 307
madcowswe 16:52250d8d8fce 308 Matrix<float, 3, 1> PH (P * trans(H));
madcowswe 16:52250d8d8fce 309 S = (H * PH)(0,0) + variance;
madcowswe 16:52250d8d8fce 310
madcowswe 16:52250d8d8fce 311 if (aborton2stddev && Y*Y > 4 * S) {
madcowswe 16:52250d8d8fce 312 statelock.unlock();
madcowswe 16:52250d8d8fce 313 continue;
madcowswe 16:52250d8d8fce 314 }
madcowswe 16:52250d8d8fce 315
madcowswe 16:52250d8d8fce 316 Matrix<float, 3, 1> K (PH * (1/S));
madcowswe 16:52250d8d8fce 317
madcowswe 16:52250d8d8fce 318 //Updating state
madcowswe 16:52250d8d8fce 319 X += col(K, 0) * Y;
madcowswe 16:52250d8d8fce 320 X(2) = rectifyAng(X(2));
madcowswe 16:52250d8d8fce 321
madcowswe 16:52250d8d8fce 322 P = (I3 - K * H) * P;
madcowswe 16:52250d8d8fce 323
madcowswe 16:52250d8d8fce 324 statelock.unlock();
madcowswe 16:52250d8d8fce 325
madcowswe 16:52250d8d8fce 326 }
madcowswe 16:52250d8d8fce 327
madcowswe 16:52250d8d8fce 328 } else {
madcowswe 16:52250d8d8fce 329 OLED4 = 1;
madcowswe 16:52250d8d8fce 330 //printf("ERROR: in updateloop, code %#x", evt);
madcowswe 16:52250d8d8fce 331 }
madcowswe 16:52250d8d8fce 332
madcowswe 16:52250d8d8fce 333 }
madcowswe 16:52250d8d8fce 334
madcowswe 16:52250d8d8fce 335 }
madcowswe 16:52250d8d8fce 336
madcowswe 16:52250d8d8fce 337 // reset kalman states
madcowswe 16:52250d8d8fce 338 void Kalman::KalmanReset()
madcowswe 16:52250d8d8fce 339 {
madcowswe 16:52250d8d8fce 340 float SonarMeasuresx1000[3];
madcowswe 16:52250d8d8fce 341 statelock.lock();
madcowswe 16:52250d8d8fce 342 SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
madcowswe 16:52250d8d8fce 343 SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
madcowswe 16:52250d8d8fce 344 SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
madcowswe 16:52250d8d8fce 345 //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
madcowswe 16:52250d8d8fce 346
madcowswe 16:52250d8d8fce 347 float d = beaconpos[2].y - beaconpos[1].y;
madcowswe 16:52250d8d8fce 348 float i = beaconpos[0].y - beaconpos[1].y;
madcowswe 16:52250d8d8fce 349 float j = beaconpos[0].x - beaconpos[1].x;
madcowswe 16:52250d8d8fce 350 float origin_x = beaconpos[1].x;
madcowswe 16:52250d8d8fce 351 float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
madcowswe 16:52250d8d8fce 352 float x_coor = origin_x +(SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
madcowswe 16:52250d8d8fce 353
madcowswe 16:52250d8d8fce 354 //statelock already locked
madcowswe 16:52250d8d8fce 355 X(0) = x_coor/1000.0f;
madcowswe 16:52250d8d8fce 356 X(1) = y_coor/1000.0f;
madcowswe 16:52250d8d8fce 357
madcowswe 16:52250d8d8fce 358
madcowswe 16:52250d8d8fce 359
madcowswe 16:52250d8d8fce 360 /* if (Colour){
madcowswe 16:52250d8d8fce 361 X(0) = 0.2;
madcowswe 16:52250d8d8fce 362 X(1) = 0.2;
madcowswe 16:52250d8d8fce 363 //X(2) = 0;
madcowswe 16:52250d8d8fce 364 }
madcowswe 16:52250d8d8fce 365 else {
madcowswe 16:52250d8d8fce 366 X(0) = 2.8;
madcowswe 16:52250d8d8fce 367 X(1) = 0.2;
madcowswe 16:52250d8d8fce 368 //X(2) = PI;
madcowswe 16:52250d8d8fce 369 }
madcowswe 16:52250d8d8fce 370 */
madcowswe 16:52250d8d8fce 371 P = 0.05, 0, 0,
madcowswe 16:52250d8d8fce 372 0, 0.05, 0,
madcowswe 16:52250d8d8fce 373 0, 0, 0.04;
madcowswe 16:52250d8d8fce 374
madcowswe 16:52250d8d8fce 375 // unlocks mutexes
madcowswe 16:52250d8d8fce 376 statelock.unlock();
madcowswe 16:52250d8d8fce 377
madcowswe 16:52250d8d8fce 378 }
madcowswe 16:52250d8d8fce 379
madcowswe 16:52250d8d8fce 380 } //Kalman Namespace
madcowswe 16:52250d8d8fce 381