Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/Kalman/Kalman.cpp@17:bafcef1c3579, 2012-04-29 (annotated)

Committer:
narshu
Date:
Sun Apr 29 00:09:35 2012 +0000
Revision:
17:bafcef1c3579
uptodate kalman lib; edit this one only!

Who changed what in which revision?

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