Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/Kalman/Kalman.cpp@16:52250d8d8fce, 2013-04-07 (annotated)

Committer:
madcowswe
Date:
Sun Apr 07 16:50:36 2013 +0000
Revision:
16:52250d8d8fce
Child:
17:6263e90bf3ba
Kalman init half done

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