Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Kalman/Kalman.cpp@4:7b7334441da9, 2012-04-26 (annotated)

Committer:
narshu
Date:
Thu Apr 26 22:05:59 2012 +0000
Revision:
4:7b7334441da9
Child:
5:7ac07bf30707
Fixed retared bugs!

Who changed what in which revision?

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