Oskar Weigl / Mbed 2 deprecated Eurobot2013

We are going to win! wohoo

Dependencies:   mbed mbed-rtos

Kalman/Kalman.cpp@9:08552997b544, 2012-11-14 (annotated)

Committer:
madcowswe
Date:
Wed Nov 14 17:15:53 2012 +0000
Revision:
9:08552997b544
Parent:
6:5a52c046d8f7 
Added an important comment

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sv 1:6799c07fe510 1 //***************************************************************************************
sv 1:6799c07fe510 2 //Kalman Filter implementation
sv 1:6799c07fe510 3 //***************************************************************************************
sv 1:6799c07fe510 4 #include "Kalman.h"
sv 1:6799c07fe510 5 #include "rtos.h"
sv 1:6799c07fe510 6 #include "RFSRF05.h"
sv 1:6799c07fe510 7 #include "math.h"
sv 1:6799c07fe510 8 #include "globals.h"
madcowswe 6:5a52c046d8f7 9 #include "encoders.h"
sv 1:6799c07fe510 10 #include "system.h"
sv 1:6799c07fe510 11 #include "geometryfuncs.h"
sv 1:6799c07fe510 12
sv 1:6799c07fe510 13 #include <tvmet/Matrix.h>
sv 1:6799c07fe510 14 #include <tvmet/Vector.h>
sv 1:6799c07fe510 15 using namespace tvmet;
sv 1:6799c07fe510 16
madcowswe 6:5a52c046d8f7 17 Kalman::Kalman(Encoders &encodersin,
sv 1:6799c07fe510 18 UI &uiin,
sv 1:6799c07fe510 19 PinName Sonar_Trig,
sv 1:6799c07fe510 20 PinName Sonar_Echo0,
sv 1:6799c07fe510 21 PinName Sonar_Echo1,
sv 1:6799c07fe510 22 PinName Sonar_Echo2,
sv 1:6799c07fe510 23 PinName Sonar_Echo3,
sv 1:6799c07fe510 24 PinName Sonar_Echo4,
sv 1:6799c07fe510 25 PinName Sonar_Echo5,
sv 1:6799c07fe510 26 PinName Sonar_SDI,
sv 1:6799c07fe510 27 PinName Sonar_SDO,
sv 1:6799c07fe510 28 PinName Sonar_SCK,
sv 1:6799c07fe510 29 PinName Sonar_NCS,
sv 1:6799c07fe510 30 PinName Sonar_NIRQ) :
sv 1:6799c07fe510 31 ir(*this),
sv 1:6799c07fe510 32 sonararray(Sonar_Trig,
sv 1:6799c07fe510 33 Sonar_Echo0,
sv 1:6799c07fe510 34 Sonar_Echo1,
sv 1:6799c07fe510 35 Sonar_Echo2,
sv 1:6799c07fe510 36 Sonar_Echo3,
sv 1:6799c07fe510 37 Sonar_Echo4,
sv 1:6799c07fe510 38 Sonar_Echo5,
sv 1:6799c07fe510 39 Sonar_SDI,
sv 1:6799c07fe510 40 Sonar_SDO,
sv 1:6799c07fe510 41 Sonar_SCK,
sv 1:6799c07fe510 42 Sonar_NCS,
sv 1:6799c07fe510 43 Sonar_NIRQ),
madcowswe 6:5a52c046d8f7 44 encoders(encodersin),
sv 1:6799c07fe510 45 ui(uiin),
sv 1:6799c07fe510 46 predictthread(predictloopwrapper, this, osPriorityNormal, 512),
sv 1:6799c07fe510 47 predictticker( SIGTICKARGS(predictthread, 0x1) ),
sv 1:6799c07fe510 48 // sonarthread(sonarloopwrapper, this, osPriorityNormal, 256),
sv 1:6799c07fe510 49 // sonarticker( SIGTICKARGS(sonarthread, 0x1) ),
sv 1:6799c07fe510 50 updatethread(updateloopwrapper, this, osPriorityNormal, 512) {
sv 1:6799c07fe510 51
sv 1:6799c07fe510 52 //Initilising offsets
sv 1:6799c07fe510 53 InitLock.lock();
sv 1:6799c07fe510 54 IR_Offset = 0;
sv 1:6799c07fe510 55 Sonar_Offset = 0;
sv 1:6799c07fe510 56 InitLock.unlock();
sv 1:6799c07fe510 57
sv 1:6799c07fe510 58
sv 1:6799c07fe510 59 //Initilising matrices
sv 1:6799c07fe510 60
sv 1:6799c07fe510 61 // X = x, y, theta;
sv 1:6799c07fe510 62 if (Colour)
sv 1:6799c07fe510 63 X = 0.5, 0, 0;
sv 1:6799c07fe510 64 else
sv 1:6799c07fe510 65 X = 2.5, 0, PI;
sv 1:6799c07fe510 66
sv 1:6799c07fe510 67 P = 1, 0, 0,
sv 1:6799c07fe510 68 0, 1, 0,
sv 1:6799c07fe510 69 0, 0, 0.04;
sv 1:6799c07fe510 70
sv 1:6799c07fe510 71 //measurment variance R is provided by each sensor when calling runupdate
sv 1:6799c07fe510 72
sv 1:6799c07fe510 73 //attach callback
sv 1:6799c07fe510 74 sonararray.callbackobj = (DummyCT*)this;
sv 1:6799c07fe510 75 sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance, float variance)) &Kalman::runupdate;
sv 1:6799c07fe510 76
sv 1:6799c07fe510 77
sv 1:6799c07fe510 78 predictticker.start(20);
sv 1:6799c07fe510 79 // sonarticker.start(50);
sv 1:6799c07fe510 80
sv 1:6799c07fe510 81 }
sv 1:6799c07fe510 82
sv 1:6799c07fe510 83
sv 1:6799c07fe510 84 //Note: this init function assumes that the robot faces east, theta=0, in the +x direction
sv 1:6799c07fe510 85 void Kalman::KalmanInit() {
madcowswe 6:5a52c046d8f7 86
sv 1:6799c07fe510 87 float SonarMeasuresx1000[3];
sv 1:6799c07fe510 88 float IRMeasuresloc[3];
sv 1:6799c07fe510 89 int beacon_cnt = 0;
sv 1:6799c07fe510 90
sv 1:6799c07fe510 91
sv 1:6799c07fe510 92 // doesn't work since they break the ISR
sv 1:6799c07fe510 93 /*
sv 1:6799c07fe510 94 #ifdef ROBOT_PRIMARY
sv 1:6799c07fe510 95 LPC_UART3->FCR = LPC_UART3->FCR | 0x06; // Flush the serial FIFO buffer / OR with FCR
sv 1:6799c07fe510 96 #else
sv 1:6799c07fe510 97 LPC_UART1->FCR = LPC_UART1->FCR | 0x06; // Flush the serial FIFO buffer / OR with FCR
sv 1:6799c07fe510 98 #endif
sv 1:6799c07fe510 99 */
sv 1:6799c07fe510 100 // zeros the measurements
sv 1:6799c07fe510 101 for (int i = 0; i < 3; i++) {
sv 1:6799c07fe510 102 SonarMeasures[i] = 0;
sv 1:6799c07fe510 103 IRMeasures[i] = 0;
sv 1:6799c07fe510 104 }
sv 1:6799c07fe510 105
sv 1:6799c07fe510 106 InitLock.lock();
sv 1:6799c07fe510 107 //zeros offsets
sv 1:6799c07fe510 108 IR_Offset = 0;
sv 1:6799c07fe510 109 Sonar_Offset = 0;
sv 1:6799c07fe510 110 InitLock.unlock();
sv 1:6799c07fe510 111
sv 1:6799c07fe510 112 // attaches ir interrup
sv 1:6799c07fe510 113 ir.attachisr();
sv 1:6799c07fe510 114
sv 1:6799c07fe510 115 //wating untill the IR has reved up and picked up some valid data
sv 1:6799c07fe510 116 //Thread::wait(1000);
sv 1:6799c07fe510 117 wait(2);
sv 1:6799c07fe510 118
sv 1:6799c07fe510 119 //temporaraly disable IR updates
sv 1:6799c07fe510 120 ir.detachisr();
sv 1:6799c07fe510 121
sv 1:6799c07fe510 122 //lock the state throughout the computation, as we will override the state at the end
sv 1:6799c07fe510 123 InitLock.lock();
sv 1:6799c07fe510 124 statelock.lock();
sv 1:6799c07fe510 125
sv 1:6799c07fe510 126
sv 1:6799c07fe510 127
sv 1:6799c07fe510 128 SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
sv 1:6799c07fe510 129 SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
sv 1:6799c07fe510 130 SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
sv 1:6799c07fe510 131 IRMeasuresloc[0] = IRMeasures[0];
sv 1:6799c07fe510 132 IRMeasuresloc[1] = IRMeasures[1];
sv 1:6799c07fe510 133 IRMeasuresloc[2] = IRMeasures[2];
sv 1:6799c07fe510 134 //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
sv 1:6799c07fe510 135
sv 1:6799c07fe510 136 float d = beaconpos[2].y - beaconpos[1].y;
sv 1:6799c07fe510 137 float i = beaconpos[0].y - beaconpos[1].y;
sv 1:6799c07fe510 138 float j = beaconpos[0].x - beaconpos[1].x;
sv 1:6799c07fe510 139 float origin_x = beaconpos[1].x;
sv 1:6799c07fe510 140 float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
sv 1:6799c07fe510 141 float x_coor = origin_x + (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
sv 1:6799c07fe510 142
sv 1:6799c07fe510 143 //debug for trilateration
sv 1:6799c07fe510 144 printf("Cal at x: %0.4f, y: %0.4f \r\n",x_coor,y_coor );
sv 1:6799c07fe510 145
sv 1:6799c07fe510 146 float Dist_Exp[3];
sv 1:6799c07fe510 147 for (int i = 0; i < 3; i++) {
sv 1:6799c07fe510 148 //Compute sonar offset
sv 1:6799c07fe510 149 Dist_Exp[i] = hypot(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
sv 1:6799c07fe510 150 Sonar_Offset += (SonarMeasuresx1000[i]-Dist_Exp[i])/3000.0f;
sv 1:6799c07fe510 151
sv 1:6799c07fe510 152 //Compute IR offset
sv 1:6799c07fe510 153 float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
sv 1:6799c07fe510 154 if (!Colour)
sv 1:6799c07fe510 155 angle_est -= PI;
sv 1:6799c07fe510 156 //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
sv 1:6799c07fe510 157 // take average offset angle from valid readings
sv 1:6799c07fe510 158 if (IRMeasuresloc[i] != 0) {
sv 1:6799c07fe510 159 beacon_cnt ++;
sv 1:6799c07fe510 160 // changed to current angle - estimated angle
sv 1:6799c07fe510 161 float angle_temp = IRMeasuresloc[i] - angle_est;
sv 1:6799c07fe510 162 angle_temp -= (floor(angle_temp/(2*PI)))*2*PI;
sv 1:6799c07fe510 163 IR_Offset += angle_temp;
sv 1:6799c07fe510 164 }
sv 1:6799c07fe510 165 }
sv 1:6799c07fe510 166 IR_Offset /= float(beacon_cnt);
sv 1:6799c07fe510 167
sv 1:6799c07fe510 168 //debug
sv 1:6799c07fe510 169 printf("Offsets IR: %0.4f, Sonar: %0.4f \r\n",IR_Offset*180/PI,Sonar_Offset*1000 );
sv 1:6799c07fe510 170
sv 1:6799c07fe510 171 //statelock already locked
sv 1:6799c07fe510 172 X(0) = x_coor/1000.0f;
sv 1:6799c07fe510 173 X(1) = y_coor/1000.0f;
sv 1:6799c07fe510 174
sv 1:6799c07fe510 175 if (Colour)
sv 1:6799c07fe510 176 X(2) = 0;
sv 1:6799c07fe510 177 else
sv 1:6799c07fe510 178 X(2) = PI;
sv 1:6799c07fe510 179
sv 1:6799c07fe510 180 // unlocks mutexes
sv 1:6799c07fe510 181 InitLock.unlock();
sv 1:6799c07fe510 182 statelock.unlock();
sv 1:6799c07fe510 183
sv 1:6799c07fe510 184
sv 1:6799c07fe510 185 //reattach the IR processing
sv 1:6799c07fe510 186 ir.attachisr();
sv 1:6799c07fe510 187 }
sv 1:6799c07fe510 188
sv 1:6799c07fe510 189
sv 1:6799c07fe510 190 void Kalman::predictloop() {
sv 1:6799c07fe510 191
sv 1:6799c07fe510 192 OLED4 = !ui.regid(0, 3);
sv 1:6799c07fe510 193 OLED4 = !ui.regid(1, 4);
sv 1:6799c07fe510 194
sv 1:6799c07fe510 195 float lastleft = 0;
sv 1:6799c07fe510 196 float lastright = 0;
sv 1:6799c07fe510 197
sv 1:6799c07fe510 198 while (1) {
sv 1:6799c07fe510 199 Thread::signal_wait(0x1);
sv 1:6799c07fe510 200 OLED1 = !OLED1;
sv 1:6799c07fe510 201
madcowswe 6:5a52c046d8f7 202 int leftenc = encoders.getEncoder1();
madcowswe 6:5a52c046d8f7 203 int rightenc = encoders.getEncoder2();
sv 1:6799c07fe510 204
madcowswe 6:5a52c046d8f7 205 float dleft = encoders.encoderToDistance(leftenc-lastleft)/1000.0f;
madcowswe 6:5a52c046d8f7 206 float dright = encoders.encoderToDistance(rightenc-lastright)/1000.0f;
sv 1:6799c07fe510 207
sv 1:6799c07fe510 208 lastleft = leftenc;
sv 1:6799c07fe510 209 lastright = rightenc;
sv 1:6799c07fe510 210
sv 1:6799c07fe510 211
sv 1:6799c07fe510 212 //The below calculation are in body frame (where +x is forward)
sv 1:6799c07fe510 213 float dxp, dyp,d,r;
sv 1:6799c07fe510 214 float thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f);
sv 1:6799c07fe510 215 if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
sv 1:6799c07fe510 216 d = (dright + dleft)/2.0f;
sv 1:6799c07fe510 217 dxp = d*cos(thetap/2.0f);
sv 1:6799c07fe510 218 dyp = d*sin(thetap/2.0f);
sv 1:6799c07fe510 219
sv 1:6799c07fe510 220 } else { //calculate circle arc
sv 1:6799c07fe510 221 //float r = (right + left) / (4.0f * PI * thetap);
sv 1:6799c07fe510 222 r = (dright + dleft) / (2.0f*thetap);
sv 1:6799c07fe510 223 dxp = abs(r)*sin(thetap);
sv 1:6799c07fe510 224 dyp = r - r*cos(thetap);
sv 1:6799c07fe510 225 }
sv 1:6799c07fe510 226
sv 1:6799c07fe510 227 statelock.lock();
sv 1:6799c07fe510 228
sv 1:6799c07fe510 229 float tempX2 = X(2);
sv 1:6799c07fe510 230 //rotating to cartesian frame and updating state
sv 1:6799c07fe510 231 X(0) += dxp * cos(X(2)) - dyp * sin(X(2));
sv 1:6799c07fe510 232 X(1) += dxp * sin(X(2)) + dyp * cos(X(2));
sv 1:6799c07fe510 233 X(2) = rectifyAng(X(2) + thetap);
sv 1:6799c07fe510 234
sv 1:6799c07fe510 235 //Linearising F around X
sv 1:6799c07fe510 236 float avgX2 = (X(2) + tempX2)/2.0f;
sv 1:6799c07fe510 237 Matrix<float, 3, 3> F;
sv 1:6799c07fe510 238 F = 1, 0, (dxp * -sin(avgX2) - dyp * cos(avgX2)),
sv 1:6799c07fe510 239 0, 1, (dxp * cos(avgX2) - dyp * sin(avgX2)),
sv 1:6799c07fe510 240 0, 0, 1;
sv 1:6799c07fe510 241
sv 1:6799c07fe510 242 //Generating forward and rotational variance
sv 1:6799c07fe510 243 float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f;
sv 1:6799c07fe510 244 float varang = varperang * abs(thetap);
sv 1:6799c07fe510 245 float varxydt = xyvarpertime * PREDICTPERIOD/1000.0f;
sv 1:6799c07fe510 246 float varangdt = angvarpertime * PREDICTPERIOD/1000.0f;
sv 1:6799c07fe510 247
sv 1:6799c07fe510 248 //Rotating into cartesian frame
sv 1:6799c07fe510 249 Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
sv 1:6799c07fe510 250 Qsub = varfwd + varxydt, 0,
sv 1:6799c07fe510 251 0, varxydt;
sv 1:6799c07fe510 252
sv 1:6799c07fe510 253 Qrot = Rotmatrix(X(2));
sv 1:6799c07fe510 254
sv 1:6799c07fe510 255 Qsubrot = Qrot * Qsub * trans(Qrot);
sv 1:6799c07fe510 256
sv 1:6799c07fe510 257 //Generate Q
sv 1:6799c07fe510 258 Matrix<float, 3, 3> Q;//(Qsubrot);
sv 1:6799c07fe510 259 Q = Qsubrot(0,0), Qsubrot(0,1), 0,
sv 1:6799c07fe510 260 Qsubrot(1,0), Qsubrot(1,1), 0,
sv 1:6799c07fe510 261 0, 0, varang + varangdt;
sv 1:6799c07fe510 262
sv 1:6799c07fe510 263 P = F * P * trans(F) + Q;
sv 1:6799c07fe510 264
sv 1:6799c07fe510 265 //Update UI
sv 1:6799c07fe510 266 float statecpy[] = {X(0), X(1), X(2)};
sv 1:6799c07fe510 267 ui.updateval(0, statecpy, 3);
sv 1:6799c07fe510 268
sv 1:6799c07fe510 269 float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
sv 1:6799c07fe510 270 ui.updateval(1, Pcpy, 4);
sv 1:6799c07fe510 271
sv 1:6799c07fe510 272 statelock.unlock();
sv 1:6799c07fe510 273 }
sv 1:6799c07fe510 274 }
sv 1:6799c07fe510 275
sv 1:6799c07fe510 276 //void Kalman::sonarloop() {
sv 1:6799c07fe510 277 // while (1) {
sv 1:6799c07fe510 278 // Thread::signal_wait(0x1);
sv 1:6799c07fe510 279 // sonararray.startRange();
sv 1:6799c07fe510 280 // }
sv 1:6799c07fe510 281 //}
sv 1:6799c07fe510 282
sv 1:6799c07fe510 283
sv 1:6799c07fe510 284 void Kalman::runupdate(measurement_t type, float value, float variance) {
sv 1:6799c07fe510 285 //printf("beacon %d dist %f\r\n", sonarid, dist);
sv 1:6799c07fe510 286 //led2 = !led2;
sv 1:6799c07fe510 287
sv 1:6799c07fe510 288 measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
sv 1:6799c07fe510 289 if (measured) {
sv 1:6799c07fe510 290 measured->mtype = type;
sv 1:6799c07fe510 291 measured->value = value;
sv 1:6799c07fe510 292 measured->variance = variance;
sv 1:6799c07fe510 293
sv 1:6799c07fe510 294 osStatus putret = measureMQ.put(measured);
sv 1:6799c07fe510 295 if (putret)
sv 1:6799c07fe510 296 OLED4 = 1;
sv 1:6799c07fe510 297 // printf("putting in MQ error code %#x\r\n", putret);
sv 1:6799c07fe510 298 } else {
sv 1:6799c07fe510 299 OLED4 = 1;
sv 1:6799c07fe510 300 //printf("MQalloc returned NULL ptr\r\n");
sv 1:6799c07fe510 301 }
sv 1:6799c07fe510 302
sv 1:6799c07fe510 303 }
sv 1:6799c07fe510 304
sv 1:6799c07fe510 305 void Kalman::updateloop() {
sv 1:6799c07fe510 306
sv 1:6799c07fe510 307 //sonar Y chanels
sv 1:6799c07fe510 308 ui.regid(2, 1);
sv 1:6799c07fe510 309 ui.regid(3, 1);
sv 1:6799c07fe510 310 ui.regid(4, 1);
sv 1:6799c07fe510 311
sv 1:6799c07fe510 312 //IR Y chanels
sv 1:6799c07fe510 313 ui.regid(5, 1);
sv 1:6799c07fe510 314 ui.regid(6, 1);
sv 1:6799c07fe510 315 ui.regid(7, 1);
sv 1:6799c07fe510 316
sv 1:6799c07fe510 317 measurement_t type;
sv 1:6799c07fe510 318 float value,variance,rbx,rby,expecdist,Y;
sv 1:6799c07fe510 319 float dhdx,dhdy;
sv 1:6799c07fe510 320 bool aborton2stddev = false;
sv 1:6799c07fe510 321
sv 1:6799c07fe510 322 Matrix<float, 1, 3> H;
sv 1:6799c07fe510 323
sv 1:6799c07fe510 324 float S;
sv 1:6799c07fe510 325 Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
sv 1:6799c07fe510 326
sv 1:6799c07fe510 327
sv 1:6799c07fe510 328 while (1) {
sv 1:6799c07fe510 329 OLED2 = !OLED2;
sv 1:6799c07fe510 330
sv 1:6799c07fe510 331 osEvent evt = measureMQ.get();
sv 1:6799c07fe510 332
sv 1:6799c07fe510 333 if (evt.status == osEventMail) {
sv 1:6799c07fe510 334
sv 1:6799c07fe510 335 measurmentdata &measured = *(measurmentdata*)evt.value.p;
sv 1:6799c07fe510 336 type = measured.mtype; //Note, may support more measurment types than sonar in the future!
sv 1:6799c07fe510 337 value = measured.value;
sv 1:6799c07fe510 338 variance = measured.variance;
sv 1:6799c07fe510 339
sv 1:6799c07fe510 340 // don't forget to free the memory
sv 1:6799c07fe510 341 measureMQ.free(&measured);
sv 1:6799c07fe510 342
sv 1:6799c07fe510 343 if (type <= maxmeasure) {
sv 1:6799c07fe510 344
sv 1:6799c07fe510 345 if (type <= SONAR3) {
sv 1:6799c07fe510 346
sv 1:6799c07fe510 347 InitLock.lock();
sv 1:6799c07fe510 348 float dist = value / 1000.0f - Sonar_Offset; //converting to m from mm,subtract the offset
sv 1:6799c07fe510 349 InitLock.unlock();
sv 1:6799c07fe510 350
sv 1:6799c07fe510 351 int sonarid = type;
sv 1:6799c07fe510 352 aborton2stddev = true;
sv 1:6799c07fe510 353
sv 1:6799c07fe510 354 statelock.lock();
sv 1:6799c07fe510 355 //update the current sonar readings
sv 1:6799c07fe510 356 SonarMeasures[sonarid] = dist;
sv 1:6799c07fe510 357
sv 1:6799c07fe510 358 rbx = X(0) - beaconpos[sonarid].x/1000.0f;
sv 1:6799c07fe510 359 rby = X(1) - beaconpos[sonarid].y/1000.0f;
sv 1:6799c07fe510 360
sv 1:6799c07fe510 361 expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
sv 1:6799c07fe510 362 Y = dist - expecdist;
sv 1:6799c07fe510 363
sv 1:6799c07fe510 364 //send to ui
sv 1:6799c07fe510 365 ui.updateval(sonarid+2, Y);
sv 1:6799c07fe510 366
sv 1:6799c07fe510 367 dhdx = rbx / expecdist;
sv 1:6799c07fe510 368 dhdy = rby / expecdist;
sv 1:6799c07fe510 369
sv 1:6799c07fe510 370 H = dhdx, dhdy, 0;
sv 1:6799c07fe510 371
sv 1:6799c07fe510 372 } else if (type <= IR3) {
sv 1:6799c07fe510 373
sv 1:6799c07fe510 374 aborton2stddev = false;
sv 1:6799c07fe510 375 int IRidx = type-3;
sv 1:6799c07fe510 376
sv 1:6799c07fe510 377 // subtract the IR offset
sv 1:6799c07fe510 378 InitLock.lock();
sv 1:6799c07fe510 379 value -= IR_Offset;
sv 1:6799c07fe510 380 InitLock.unlock();
sv 1:6799c07fe510 381
sv 1:6799c07fe510 382 statelock.lock();
sv 1:6799c07fe510 383 IRMeasures[IRidx] = value;
sv 1:6799c07fe510 384
sv 1:6799c07fe510 385 rbx = X(0) - beaconpos[IRidx].x/1000.0f;
sv 1:6799c07fe510 386 rby = X(1) - beaconpos[IRidx].y/1000.0f;
sv 1:6799c07fe510 387
sv 1:6799c07fe510 388 float expecang = atan2(-rby, -rbx) - X(2);
sv 1:6799c07fe510 389 Y = rectifyAng(value - expecang);
sv 1:6799c07fe510 390
sv 1:6799c07fe510 391 //send to ui
sv 1:6799c07fe510 392 ui.updateval(IRidx + 5, Y);
sv 1:6799c07fe510 393
sv 1:6799c07fe510 394 float dstsq = rbx*rbx + rby*rby;
sv 1:6799c07fe510 395 H = -rby/dstsq, rbx/dstsq, -1;
sv 1:6799c07fe510 396 }
sv 1:6799c07fe510 397
sv 1:6799c07fe510 398 Matrix<float, 3, 1> PH (P * trans(H));
sv 1:6799c07fe510 399 S = (H * PH)(0,0) + variance;
sv 1:6799c07fe510 400
sv 1:6799c07fe510 401 if (aborton2stddev && Y*Y > 4 * S) {
sv 1:6799c07fe510 402 statelock.unlock();
sv 1:6799c07fe510 403 continue;
sv 1:6799c07fe510 404 }
sv 1:6799c07fe510 405
sv 1:6799c07fe510 406 Matrix<float, 3, 1> K (PH * (1/S));
sv 1:6799c07fe510 407
sv 1:6799c07fe510 408 //Updating state
sv 1:6799c07fe510 409 X += col(K, 0) * Y;
sv 1:6799c07fe510 410 X(2) = rectifyAng(X(2));
sv 1:6799c07fe510 411
sv 1:6799c07fe510 412 P = (I3 - K * H) * P;
sv 1:6799c07fe510 413
sv 1:6799c07fe510 414 statelock.unlock();
sv 1:6799c07fe510 415
sv 1:6799c07fe510 416 }
sv 1:6799c07fe510 417
sv 1:6799c07fe510 418 } else {
sv 1:6799c07fe510 419 OLED4 = 1;
sv 1:6799c07fe510 420 //printf("ERROR: in updateloop, code %#x", evt);
sv 1:6799c07fe510 421 }
sv 1:6799c07fe510 422
sv 1:6799c07fe510 423 }
sv 1:6799c07fe510 424
sv 1:6799c07fe510 425 }
sv 1:6799c07fe510 426
sv 1:6799c07fe510 427 // reset kalman states
sv 1:6799c07fe510 428 void Kalman::KalmanReset() {
sv 1:6799c07fe510 429 float SonarMeasuresx1000[3];
sv 1:6799c07fe510 430 statelock.lock();
sv 1:6799c07fe510 431 SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f;
sv 1:6799c07fe510 432 SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f;
sv 1:6799c07fe510 433 SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f;
sv 1:6799c07fe510 434 //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
sv 1:6799c07fe510 435
sv 1:6799c07fe510 436 float d = beaconpos[2].y - beaconpos[1].y;
sv 1:6799c07fe510 437 float i = beaconpos[0].y - beaconpos[1].y;
sv 1:6799c07fe510 438 float j = beaconpos[0].x - beaconpos[1].x;
sv 1:6799c07fe510 439 float origin_x = beaconpos[1].x;
sv 1:6799c07fe510 440 float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d);
sv 1:6799c07fe510 441 float x_coor = origin_x +(SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j;
sv 1:6799c07fe510 442
sv 1:6799c07fe510 443 //statelock already locked
sv 1:6799c07fe510 444 X(0) = x_coor/1000.0f;
sv 1:6799c07fe510 445 X(1) = y_coor/1000.0f;
sv 1:6799c07fe510 446
sv 1:6799c07fe510 447
sv 1:6799c07fe510 448
sv 1:6799c07fe510 449 /* if (Colour){
sv 1:6799c07fe510 450 X(0) = 0.2;
sv 1:6799c07fe510 451 X(1) = 0.2;
sv 1:6799c07fe510 452 //X(2) = 0;
sv 1:6799c07fe510 453 }
sv 1:6799c07fe510 454 else {
sv 1:6799c07fe510 455 X(0) = 2.8;
sv 1:6799c07fe510 456 X(1) = 0.2;
sv 1:6799c07fe510 457 //X(2) = PI;
sv 1:6799c07fe510 458 }
sv 1:6799c07fe510 459 */
sv 1:6799c07fe510 460 P = 0.05, 0, 0,
sv 1:6799c07fe510 461 0, 0.05, 0,
sv 1:6799c07fe510 462 0, 0, 0.04;
sv 1:6799c07fe510 463
sv 1:6799c07fe510 464 // unlocks mutexes
sv 1:6799c07fe510 465 statelock.unlock();
sv 1:6799c07fe510 466
sv 1:6799c07fe510 467 }