Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/Kalman/Kalman.cpp@20:70d651156779, 2013-04-09 (annotated)

Committer:
madcowswe
Date:
Tue Apr 09 15:33:36 2013 +0000
Revision:
20:70d651156779
Parent:
19:4b993a9a156e
Child:
21:167dacfe0b14
Predict loop running, update loop not done.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
madcowswe 16:52250d8d8fce 1 //***************************************************************************************
madcowswe 16:52250d8d8fce 2 //Kalman Filter implementation
madcowswe 16:52250d8d8fce 3 //***************************************************************************************
madcowswe 16:52250d8d8fce 4 #include "Kalman.h"
madcowswe 16:52250d8d8fce 5 #include "rtos.h"
madcowswe 16:52250d8d8fce 6 #include "math.h"
madcowswe 16:52250d8d8fce 7 #include "supportfuncs.h"
madcowswe 20:70d651156779 8 #include "Encoder.h"
madcowswe 16:52250d8d8fce 9 //#include "globals.h"
madcowswe 16:52250d8d8fce 10
madcowswe 20:70d651156779 11 #include "tvmet/Matrix.h"
madcowswe 16:52250d8d8fce 12 using namespace tvmet;
madcowswe 16:52250d8d8fce 13
madcowswe 16:52250d8d8fce 14
madcowswe 16:52250d8d8fce 15
madcowswe 16:52250d8d8fce 16 namespace Kalman
madcowswe 16:52250d8d8fce 17 {
madcowswe 16:52250d8d8fce 18
madcowswe 20:70d651156779 19 Ticker predictticker;
madcowswe 20:70d651156779 20
madcowswe 20:70d651156779 21 DigitalOut OLED4(LED4);
madcowswe 20:70d651156779 22 DigitalOut OLED1(LED1);
madcowswe 20:70d651156779 23
madcowswe 16:52250d8d8fce 24 //State variables
madcowswe 19:4b993a9a156e 25 Matrix<float, 3, 1> X;
madcowswe 16:52250d8d8fce 26 Matrix<float, 3, 3> P;
madcowswe 16:52250d8d8fce 27 Mutex statelock;
madcowswe 16:52250d8d8fce 28
madcowswe 16:52250d8d8fce 29 float RawReadings[maxmeasure+1];
madcowswe 19:4b993a9a156e 30 float IRpahseOffset;
madcowswe 16:52250d8d8fce 31
madcowswe 20:70d651156779 32 bool Kalman_inited = 0;
madcowswe 16:52250d8d8fce 33
madcowswe 16:52250d8d8fce 34 struct measurmentdata {
madcowswe 16:52250d8d8fce 35 measurement_t mtype;
madcowswe 16:52250d8d8fce 36 float value;
madcowswe 16:52250d8d8fce 37 float variance;
madcowswe 19:4b993a9a156e 38 };
madcowswe 16:52250d8d8fce 39
madcowswe 16:52250d8d8fce 40 Mail <measurmentdata, 16> measureMQ;
madcowswe 16:52250d8d8fce 41
madcowswe 20:70d651156779 42 Thread* predict_thread_ptr = NULL;
madcowswe 16:52250d8d8fce 43
madcowswe 16:52250d8d8fce 44
madcowswe 16:52250d8d8fce 45 //Note: this init function assumes that the robot faces east, theta=0, in the +x direction
madcowswe 16:52250d8d8fce 46 void KalmanInit()
madcowswe 16:52250d8d8fce 47 {
madcowswe 20:70d651156779 48 printf("kalmaninit \r\n");
madcowswe 20:70d651156779 49
madcowswe 20:70d651156779 50 //WARNING: HARDCODED!
madcowswe 20:70d651156779 51
madcowswe 16:52250d8d8fce 52 //solve for our position (assume perfect bias)
madcowswe 20:70d651156779 53 const float d = beaconpos[2].y - beaconpos[1].y;
madcowswe 20:70d651156779 54 const float i = beaconpos[2].y - beaconpos[0].y;
madcowswe 20:70d651156779 55 const float j = beaconpos[2].x - beaconpos[0].x;
madcowswe 20:70d651156779 56 float r1 = RawReadings[SONAR2];
madcowswe 19:4b993a9a156e 57 float r2 = RawReadings[SONAR1];
madcowswe 20:70d651156779 58 float r3 = RawReadings[SONAR0];
madcowswe 20:70d651156779 59
madcowswe 20:70d651156779 60 printf("ranges: 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", r1, r2, r3);
madcowswe 17:6263e90bf3ba 61
madcowswe 19:4b993a9a156e 62 float y_coor = (r1*r1-r2*r2+d*d)/(2*d);
madcowswe 17:6263e90bf3ba 63 float x_coor = (r1*r1-r3*r3+i*i+j*j)/(2*j) - (i*y_coor)/j;
madcowswe 20:70d651156779 64
madcowswe 20:70d651156779 65 //coordinate system hack (for now)
madcowswe 20:70d651156779 66 x_coor = beaconpos[2].x - x_coor;
madcowswe 20:70d651156779 67 y_coor = beaconpos[2].y - y_coor;
madcowswe 20:70d651156779 68
madcowswe 20:70d651156779 69 printf("solved pos from sonar: %f, %f \r\n", x_coor, y_coor);
madcowswe 20:70d651156779 70
madcowswe 16:52250d8d8fce 71 //IR
madcowswe 16:52250d8d8fce 72 float IRMeasuresloc[3];
madcowswe 16:52250d8d8fce 73 IRMeasuresloc[0] = RawReadings[IR0];
madcowswe 16:52250d8d8fce 74 IRMeasuresloc[1] = RawReadings[IR1];
madcowswe 16:52250d8d8fce 75 IRMeasuresloc[2] = RawReadings[IR2];
madcowswe 20:70d651156779 76 printf("IR 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
madcowswe 16:52250d8d8fce 77
madcowswe 17:6263e90bf3ba 78 float IR_Offsets[3];
madcowswe 17:6263e90bf3ba 79 float fromb0offset = 0;
madcowswe 16:52250d8d8fce 80 for (int i = 0; i < 3; i++) {
madcowswe 16:52250d8d8fce 81
madcowswe 16:52250d8d8fce 82 //Compute IR offset
madcowswe 16:52250d8d8fce 83 float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
madcowswe 17:6263e90bf3ba 84
madcowswe 16:52250d8d8fce 85 //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
madcowswe 19:4b993a9a156e 86 IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
madcowswe 20:70d651156779 87
madcowswe 19:4b993a9a156e 88 fromb0offset += constrainAngle(IR_Offsets[i] - IR_Offsets[0]);
madcowswe 16:52250d8d8fce 89 }
madcowswe 20:70d651156779 90
madcowswe 19:4b993a9a156e 91 IRpahseOffset = constrainAngle(IR_Offsets[0] + fromb0offset/3);
madcowswe 16:52250d8d8fce 92
madcowswe 16:52250d8d8fce 93 //debug
madcowswe 19:4b993a9a156e 94 printf("Offsets IR: %0.4f\r\n",IRpahseOffset*180/PI);
madcowswe 16:52250d8d8fce 95
madcowswe 16:52250d8d8fce 96 statelock.lock();
madcowswe 19:4b993a9a156e 97 X(0,0) = x_coor;
madcowswe 19:4b993a9a156e 98 X(1,0) = y_coor;
madcowswe 19:4b993a9a156e 99 X(2,0) = 0;
madcowswe 16:52250d8d8fce 100 statelock.unlock();
madcowswe 20:70d651156779 101
madcowswe 20:70d651156779 102 Kalman_inited = 1;
madcowswe 16:52250d8d8fce 103 }
madcowswe 16:52250d8d8fce 104
madcowswe 20:70d651156779 105
madcowswe 20:70d651156779 106 State getState(){
madcowswe 20:70d651156779 107 statelock.lock();
madcowswe 20:70d651156779 108 State state = {X(0,0), X(1,0), X(2,0)};
madcowswe 20:70d651156779 109 statelock.unlock();
madcowswe 20:70d651156779 110 return state;
madcowswe 20:70d651156779 111 }
madcowswe 20:70d651156779 112
madcowswe 20:70d651156779 113
madcowswe 20:70d651156779 114 void predictloop(void const *dummy)
madcowswe 16:52250d8d8fce 115 {
madcowswe 16:52250d8d8fce 116
madcowswe 20:70d651156779 117 //OLED4 = !ui.regid(0, 3);
madcowswe 20:70d651156779 118 //OLED4 = !ui.regid(1, 4);
madcowswe 16:52250d8d8fce 119
madcowswe 16:52250d8d8fce 120 float lastleft = 0;
madcowswe 16:52250d8d8fce 121 float lastright = 0;
madcowswe 16:52250d8d8fce 122
madcowswe 16:52250d8d8fce 123 while (1) {
madcowswe 16:52250d8d8fce 124 Thread::signal_wait(0x1);
madcowswe 16:52250d8d8fce 125 OLED1 = !OLED1;
madcowswe 16:52250d8d8fce 126
madcowswe 20:70d651156779 127 float leftenc = left_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 20:70d651156779 128 float rightenc = right_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 16:52250d8d8fce 129
madcowswe 20:70d651156779 130 float dleft = leftenc-lastleft;
madcowswe 20:70d651156779 131 float dright = rightenc-lastright;
madcowswe 16:52250d8d8fce 132
madcowswe 16:52250d8d8fce 133 lastleft = leftenc;
madcowswe 16:52250d8d8fce 134 lastright = rightenc;
madcowswe 16:52250d8d8fce 135
madcowswe 16:52250d8d8fce 136
madcowswe 16:52250d8d8fce 137 //The below calculation are in body frame (where +x is forward)
madcowswe 16:52250d8d8fce 138 float dxp, dyp,d,r;
madcowswe 20:70d651156779 139 float thetap = (dright - dleft) / ENCODER_WHEELBASE;
madcowswe 20:70d651156779 140 if (abs(thetap) < 0.01f) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error
madcowswe 16:52250d8d8fce 141 d = (dright + dleft)/2.0f;
madcowswe 16:52250d8d8fce 142 dxp = d*cos(thetap/2.0f);
madcowswe 16:52250d8d8fce 143 dyp = d*sin(thetap/2.0f);
madcowswe 16:52250d8d8fce 144
madcowswe 16:52250d8d8fce 145 } else { //calculate circle arc
madcowswe 16:52250d8d8fce 146 //float r = (right + left) / (4.0f * PI * thetap);
madcowswe 16:52250d8d8fce 147 r = (dright + dleft) / (2.0f*thetap);
madcowswe 20:70d651156779 148 dxp = r*sin(thetap);
madcowswe 16:52250d8d8fce 149 dyp = r - r*cos(thetap);
madcowswe 16:52250d8d8fce 150 }
madcowswe 16:52250d8d8fce 151
madcowswe 16:52250d8d8fce 152 statelock.lock();
madcowswe 16:52250d8d8fce 153
madcowswe 20:70d651156779 154 float tempX2 = X(2,0);
madcowswe 16:52250d8d8fce 155 //rotating to cartesian frame and updating state
madcowswe 20:70d651156779 156 X(0,0) += dxp * cos(X(2,0)) - dyp * sin(X(2,0));
madcowswe 20:70d651156779 157 X(1,0) += dxp * sin(X(2,0)) + dyp * cos(X(2,0));
madcowswe 20:70d651156779 158 X(2,0) = constrainAngle(X(2,0) + thetap);
madcowswe 16:52250d8d8fce 159
madcowswe 16:52250d8d8fce 160 //Linearising F around X
madcowswe 20:70d651156779 161 float avgX2 = (X(2,0) + tempX2)/2.0f;
madcowswe 16:52250d8d8fce 162 Matrix<float, 3, 3> F;
madcowswe 16:52250d8d8fce 163 F = 1, 0, (dxp * -sin(avgX2) - dyp * cos(avgX2)),
madcowswe 16:52250d8d8fce 164 0, 1, (dxp * cos(avgX2) - dyp * sin(avgX2)),
madcowswe 16:52250d8d8fce 165 0, 0, 1;
madcowswe 16:52250d8d8fce 166
madcowswe 16:52250d8d8fce 167 //Generating forward and rotational variance
madcowswe 16:52250d8d8fce 168 float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f;
madcowswe 16:52250d8d8fce 169 float varang = varperang * abs(thetap);
madcowswe 20:70d651156779 170 float varxydt = xyvarpertime * KALMAN_PREDICT_PERIOD;
madcowswe 20:70d651156779 171 float varangdt = angvarpertime * KALMAN_PREDICT_PERIOD;
madcowswe 16:52250d8d8fce 172
madcowswe 16:52250d8d8fce 173 //Rotating into cartesian frame
madcowswe 16:52250d8d8fce 174 Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
madcowswe 16:52250d8d8fce 175 Qsub = varfwd + varxydt, 0,
madcowswe 16:52250d8d8fce 176 0, varxydt;
madcowswe 16:52250d8d8fce 177
madcowswe 20:70d651156779 178 Qrot = Rotmatrix(X(2,0));
madcowswe 16:52250d8d8fce 179
madcowswe 16:52250d8d8fce 180 Qsubrot = Qrot * Qsub * trans(Qrot);
madcowswe 16:52250d8d8fce 181
madcowswe 16:52250d8d8fce 182 //Generate Q
madcowswe 16:52250d8d8fce 183 Matrix<float, 3, 3> Q;//(Qsubrot);
madcowswe 16:52250d8d8fce 184 Q = Qsubrot(0,0), Qsubrot(0,1), 0,
madcowswe 16:52250d8d8fce 185 Qsubrot(1,0), Qsubrot(1,1), 0,
madcowswe 16:52250d8d8fce 186 0, 0, varang + varangdt;
madcowswe 16:52250d8d8fce 187
madcowswe 16:52250d8d8fce 188 P = F * P * trans(F) + Q;
madcowswe 16:52250d8d8fce 189
madcowswe 20:70d651156779 190 //printf("x: %f, y: %f, t: %f\r\n", X(0,0), X(1,0), X(2,0));
madcowswe 16:52250d8d8fce 191 //Update UI
madcowswe 20:70d651156779 192 //float statecpy[] = {X(0,0), X(1,0), X(2,0)};
madcowswe 20:70d651156779 193 //ui.updateval(0, statecpy, 3);
madcowswe 16:52250d8d8fce 194
madcowswe 20:70d651156779 195 //float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
madcowswe 20:70d651156779 196 //ui.updateval(1, Pcpy, 4);
madcowswe 16:52250d8d8fce 197
madcowswe 16:52250d8d8fce 198 statelock.unlock();
madcowswe 16:52250d8d8fce 199 }
madcowswe 16:52250d8d8fce 200 }
madcowswe 16:52250d8d8fce 201
madcowswe 20:70d651156779 202
madcowswe 20:70d651156779 203 void predict_event_setter(){
madcowswe 20:70d651156779 204 if(predict_thread_ptr)
madcowswe 20:70d651156779 205 predict_thread_ptr->signal_set(0x1);
madcowswe 20:70d651156779 206 else
madcowswe 20:70d651156779 207 OLED4 = 1;
madcowswe 20:70d651156779 208 }
madcowswe 20:70d651156779 209
madcowswe 20:70d651156779 210 void start_predict_ticker(Thread* predict_thread_ptr_in){
madcowswe 20:70d651156779 211 predict_thread_ptr = predict_thread_ptr_in;
madcowswe 20:70d651156779 212 predictticker.attach(predict_event_setter, KALMAN_PREDICT_PERIOD);
madcowswe 20:70d651156779 213 }
madcowswe 20:70d651156779 214
madcowswe 20:70d651156779 215 void runupdate(measurement_t type, float value, float variance)
madcowswe 16:52250d8d8fce 216 {
madcowswe 20:70d651156779 217 if (!Kalman_inited) {
madcowswe 16:52250d8d8fce 218 RawReadings[type] = value;
madcowswe 20:70d651156779 219 } else {
madcowswe 17:6263e90bf3ba 220
madcowswe 20:70d651156779 221 if (type >= IR0 && type <= IR2)
madcowswe 20:70d651156779 222 RawReadings[type] = value - IRpahseOffset;
madcowswe 20:70d651156779 223 else
madcowswe 20:70d651156779 224 RawReadings[type] = value;
madcowswe 20:70d651156779 225
madcowswe 17:6263e90bf3ba 226
madcowswe 16:52250d8d8fce 227 measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
madcowswe 16:52250d8d8fce 228 if (measured) {
madcowswe 16:52250d8d8fce 229 measured->mtype = type;
madcowswe 16:52250d8d8fce 230 measured->value = value;
madcowswe 16:52250d8d8fce 231 measured->variance = variance;
madcowswe 16:52250d8d8fce 232
madcowswe 16:52250d8d8fce 233 osStatus putret = measureMQ.put(measured);
madcowswe 20:70d651156779 234 //if (putret)
madcowswe 20:70d651156779 235 //OLED4 = 1;
madcowswe 16:52250d8d8fce 236 // printf("putting in MQ error code %#x\r\n", putret);
madcowswe 16:52250d8d8fce 237 } else {
madcowswe 20:70d651156779 238 //OLED4 = 1;
madcowswe 16:52250d8d8fce 239 //printf("MQalloc returned NULL ptr\r\n");
madcowswe 16:52250d8d8fce 240 }
madcowswe 20:70d651156779 241
madcowswe 16:52250d8d8fce 242 }
madcowswe 20:70d651156779 243
madcowswe 16:52250d8d8fce 244
madcowswe 16:52250d8d8fce 245 }
madcowswe 20:70d651156779 246 /*
madcowswe 20:70d651156779 247 void Kalman::updateloop(void const *dummy)
madcowswe 16:52250d8d8fce 248 {
madcowswe 16:52250d8d8fce 249
madcowswe 16:52250d8d8fce 250 //sonar Y chanels
madcowswe 16:52250d8d8fce 251 ui.regid(2, 1);
madcowswe 16:52250d8d8fce 252 ui.regid(3, 1);
madcowswe 16:52250d8d8fce 253 ui.regid(4, 1);
madcowswe 16:52250d8d8fce 254
madcowswe 16:52250d8d8fce 255 //IR Y chanels
madcowswe 16:52250d8d8fce 256 ui.regid(5, 1);
madcowswe 16:52250d8d8fce 257 ui.regid(6, 1);
madcowswe 16:52250d8d8fce 258 ui.regid(7, 1);
madcowswe 16:52250d8d8fce 259
madcowswe 16:52250d8d8fce 260 measurement_t type;
madcowswe 16:52250d8d8fce 261 float value,variance,rbx,rby,expecdist,Y;
madcowswe 16:52250d8d8fce 262 float dhdx,dhdy;
madcowswe 16:52250d8d8fce 263 bool aborton2stddev = false;
madcowswe 16:52250d8d8fce 264
madcowswe 16:52250d8d8fce 265 Matrix<float, 1, 3> H;
madcowswe 16:52250d8d8fce 266
madcowswe 16:52250d8d8fce 267 float S;
madcowswe 16:52250d8d8fce 268 Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
madcowswe 16:52250d8d8fce 269
madcowswe 16:52250d8d8fce 270
madcowswe 16:52250d8d8fce 271 while (1) {
madcowswe 16:52250d8d8fce 272 OLED2 = !OLED2;
madcowswe 16:52250d8d8fce 273
madcowswe 16:52250d8d8fce 274 osEvent evt = measureMQ.get();
madcowswe 16:52250d8d8fce 275
madcowswe 16:52250d8d8fce 276 if (evt.status == osEventMail) {
madcowswe 16:52250d8d8fce 277
madcowswe 16:52250d8d8fce 278 measurmentdata &measured = *(measurmentdata*)evt.value.p;
madcowswe 16:52250d8d8fce 279 type = measured.mtype; //Note, may support more measurment types than sonar in the future!
madcowswe 16:52250d8d8fce 280 value = measured.value;
madcowswe 16:52250d8d8fce 281 variance = measured.variance;
madcowswe 16:52250d8d8fce 282
madcowswe 16:52250d8d8fce 283 // don't forget to free the memory
madcowswe 16:52250d8d8fce 284 measureMQ.free(&measured);
madcowswe 16:52250d8d8fce 285
madcowswe 16:52250d8d8fce 286 if (type <= maxmeasure) {
madcowswe 16:52250d8d8fce 287
madcowswe 16:52250d8d8fce 288 if (type <= SONAR3) {
madcowswe 16:52250d8d8fce 289
madcowswe 16:52250d8d8fce 290 InitLock.lock();
madcowswe 16:52250d8d8fce 291 float dist = value / 1000.0f - Sonar_Offset; //converting to m from mm,subtract the offset
madcowswe 16:52250d8d8fce 292 InitLock.unlock();
madcowswe 16:52250d8d8fce 293
madcowswe 16:52250d8d8fce 294 int sonarid = type;
madcowswe 16:52250d8d8fce 295 aborton2stddev = true;
madcowswe 16:52250d8d8fce 296
madcowswe 16:52250d8d8fce 297 statelock.lock();
madcowswe 16:52250d8d8fce 298 //update the current sonar readings
madcowswe 16:52250d8d8fce 299 SonarMeasures[sonarid] = dist;
madcowswe 16:52250d8d8fce 300
madcowswe 16:52250d8d8fce 301 rbx = X(0) - beaconpos[sonarid].x/1000.0f;
madcowswe 16:52250d8d8fce 302 rby = X(1) - beaconpos[sonarid].y/1000.0f;
madcowswe 16:52250d8d8fce 303
madcowswe 16:52250d8d8fce 304 expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
madcowswe 16:52250d8d8fce 305 Y = dist - expecdist;
madcowswe 16:52250d8d8fce 306
madcowswe 16:52250d8d8fce 307 //send to ui
madcowswe 16:52250d8d8fce 308 ui.updateval(sonarid+2, Y);
madcowswe 16:52250d8d8fce 309
madcowswe 16:52250d8d8fce 310 dhdx = rbx / expecdist;
madcowswe 16:52250d8d8fce 311 dhdy = rby / expecdist;
madcowswe 16:52250d8d8fce 312
madcowswe 16:52250d8d8fce 313 H = dhdx, dhdy, 0;
madcowswe 16:52250d8d8fce 314
madcowswe 16:52250d8d8fce 315 } else if (type <= IR3) {
madcowswe 16:52250d8d8fce 316
madcowswe 16:52250d8d8fce 317 aborton2stddev = false;
madcowswe 16:52250d8d8fce 318 int IRidx = type-3;
madcowswe 16:52250d8d8fce 319
madcowswe 16:52250d8d8fce 320 // subtract the IR offset
madcowswe 16:52250d8d8fce 321 InitLock.lock();
madcowswe 16:52250d8d8fce 322 value -= IR_Offset;
madcowswe 16:52250d8d8fce 323 InitLock.unlock();
madcowswe 16:52250d8d8fce 324
madcowswe 16:52250d8d8fce 325 statelock.lock();
madcowswe 16:52250d8d8fce 326 IRMeasures[IRidx] = value;
madcowswe 16:52250d8d8fce 327
madcowswe 16:52250d8d8fce 328 rbx = X(0) - beaconpos[IRidx].x/1000.0f;
madcowswe 16:52250d8d8fce 329 rby = X(1) - beaconpos[IRidx].y/1000.0f;
madcowswe 16:52250d8d8fce 330
madcowswe 16:52250d8d8fce 331 float expecang = atan2(-rby, -rbx) - X(2);
madcowswe 16:52250d8d8fce 332 Y = rectifyAng(value - expecang);
madcowswe 16:52250d8d8fce 333
madcowswe 16:52250d8d8fce 334 //send to ui
madcowswe 16:52250d8d8fce 335 ui.updateval(IRidx + 5, Y);
madcowswe 16:52250d8d8fce 336
madcowswe 16:52250d8d8fce 337 float dstsq = rbx*rbx + rby*rby;
madcowswe 16:52250d8d8fce 338 H = -rby/dstsq, rbx/dstsq, -1;
madcowswe 16:52250d8d8fce 339 }
madcowswe 16:52250d8d8fce 340
madcowswe 16:52250d8d8fce 341 Matrix<float, 3, 1> PH (P * trans(H));
madcowswe 16:52250d8d8fce 342 S = (H * PH)(0,0) + variance;
madcowswe 16:52250d8d8fce 343
madcowswe 16:52250d8d8fce 344 if (aborton2stddev && Y*Y > 4 * S) {
madcowswe 16:52250d8d8fce 345 statelock.unlock();
madcowswe 16:52250d8d8fce 346 continue;
madcowswe 16:52250d8d8fce 347 }
madcowswe 16:52250d8d8fce 348
madcowswe 16:52250d8d8fce 349 Matrix<float, 3, 1> K (PH * (1/S));
madcowswe 16:52250d8d8fce 350
madcowswe 16:52250d8d8fce 351 //Updating state
madcowswe 16:52250d8d8fce 352 X += col(K, 0) * Y;
madcowswe 16:52250d8d8fce 353 X(2) = rectifyAng(X(2));
madcowswe 16:52250d8d8fce 354
madcowswe 16:52250d8d8fce 355 P = (I3 - K * H) * P;
madcowswe 16:52250d8d8fce 356
madcowswe 16:52250d8d8fce 357 statelock.unlock();
madcowswe 16:52250d8d8fce 358
madcowswe 16:52250d8d8fce 359 }
madcowswe 16:52250d8d8fce 360
madcowswe 16:52250d8d8fce 361 } else {
madcowswe 16:52250d8d8fce 362 OLED4 = 1;
madcowswe 16:52250d8d8fce 363 //printf("ERROR: in updateloop, code %#x", evt);
madcowswe 16:52250d8d8fce 364 }
madcowswe 16:52250d8d8fce 365
madcowswe 16:52250d8d8fce 366 }
madcowswe 16:52250d8d8fce 367
madcowswe 16:52250d8d8fce 368 }
madcowswe 16:52250d8d8fce 369
madcowswe 19:4b993a9a156e 370 */
madcowswe 19:4b993a9a156e 371
madcowswe 16:52250d8d8fce 372 } //Kalman Namespace