Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/Kalman/Kalman.cpp@22:7ba09c0af0d0, 2012-05-03 (annotated)

Committer:
narshu
Date:
Thu May 03 14:20:04 2012 +0000
Revision:
22:7ba09c0af0d0
added 90sec timer and tigger

Who changed what in which revision?

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