Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/Kalman/Kalman.cpp@9:377560539b74, 2012-04-28 (annotated)

Committer:
narshu
Date:
Sat Apr 28 17:21:24 2012 +0000
Revision:
9:377560539b74
Restructured project to have a single shared lib; Also raised the RF baud rate

Who changed what in which revision?

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