Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/Kalman/Kalman.cpp@92:4a1225fbb146, 2013-10-15 (annotated)

Committer:
rsavitski
Date:
Tue Oct 15 12:19:32 2013 +0000
Revision:
92:4a1225fbb146
Parent:
85:b0858346d838 
touch: ripped out 2013-specific bits. Need to address "2014" comments. Rewrite AI layer and other deleted parts.

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 21:167dacfe0b14 9 #include "globals.h"
madcowswe 21:167dacfe0b14 10 #include "Printing.h"
madcowswe 16:52250d8d8fce 11
madcowswe 20:70d651156779 12 #include "tvmet/Matrix.h"
madcowswe 16:52250d8d8fce 13 using namespace tvmet;
madcowswe 16:52250d8d8fce 14
madcowswe 16:52250d8d8fce 15
madcowswe 16:52250d8d8fce 16
madcowswe 16:52250d8d8fce 17 namespace Kalman
madcowswe 16:52250d8d8fce 18 {
madcowswe 16:52250d8d8fce 19
madcowswe 20:70d651156779 20 Ticker predictticker;
madcowswe 20:70d651156779 21
rsavitski 38:c9058a401410 22 DigitalOut OLED4(LED4);
madcowswe 48:254b124cef02 23 DigitalOut OLED3(LED3);
rsavitski 38:c9058a401410 24 DigitalOut OLED1(LED1);
madcowswe 26:7cb3a21d9a2e 25 DigitalOut OLED2(LED2);
madcowswe 20:70d651156779 26
madcowswe 16:52250d8d8fce 27 //State variables
madcowswe 48:254b124cef02 28 Matrix<float, 4, 1> X;
madcowswe 48:254b124cef02 29 Matrix<float, 4, 4> P;
madcowswe 16:52250d8d8fce 30 Mutex statelock;
madcowswe 16:52250d8d8fce 31
madcowswe 16:52250d8d8fce 32 float RawReadings[maxmeasure+1];
madcowswe 49:665bdca0f2cd 33 volatile int sensorseenflags = 0;
madcowswe 16:52250d8d8fce 34
madcowswe 20:70d651156779 35 bool Kalman_inited = 0;
madcowswe 16:52250d8d8fce 36
madcowswe 16:52250d8d8fce 37 struct measurmentdata {
madcowswe 16:52250d8d8fce 38 measurement_t mtype;
madcowswe 16:52250d8d8fce 39 float value;
madcowswe 16:52250d8d8fce 40 float variance;
madcowswe 19:4b993a9a156e 41 };
madcowswe 16:52250d8d8fce 42
madcowswe 16:52250d8d8fce 43 Mail <measurmentdata, 16> measureMQ;
madcowswe 16:52250d8d8fce 44
madcowswe 20:70d651156779 45 Thread* predict_thread_ptr = NULL;
madcowswe 16:52250d8d8fce 46
madcowswe 16:52250d8d8fce 47
madcowswe 16:52250d8d8fce 48 //Note: this init function assumes that the robot faces east, theta=0, in the +x direction
madcowswe 16:52250d8d8fce 49 void KalmanInit()
madcowswe 16:52250d8d8fce 50 {
madcowswe 20:70d651156779 51 printf("kalmaninit \r\n");
madcowswe 48:254b124cef02 52
madcowswe 29:00e1493b44f0 53 //WARNING: HARDCODED! TODO: fix it so it works for both sides!
madcowswe 48:254b124cef02 54
madcowswe 31:ada943ecaceb 55 printf("waiting for all sonar, and at least 1 IR\r\n");
madcowswe 31:ada943ecaceb 56 while( ((sensorseenflags & 0x7)^0x7) || !(sensorseenflags & 0x7<<3) );
madcowswe 48:254b124cef02 57
madcowswe 85:b0858346d838 58 //#ifdef TEAM_RED
madcowswe 16:52250d8d8fce 59 //solve for our position (assume perfect bias)
madcowswe 20:70d651156779 60 const float d = beaconpos[2].y - beaconpos[1].y;
madcowswe 20:70d651156779 61 const float i = beaconpos[2].y - beaconpos[0].y;
madcowswe 20:70d651156779 62 const float j = beaconpos[2].x - beaconpos[0].x;
madcowswe 20:70d651156779 63 float r1 = RawReadings[SONAR2];
madcowswe 19:4b993a9a156e 64 float r2 = RawReadings[SONAR1];
madcowswe 20:70d651156779 65 float r3 = RawReadings[SONAR0];
madcowswe 85:b0858346d838 66 //#endif
madcowswe 85:b0858346d838 67 /*
madcowswe 72:7996aa8286ae 68 #ifdef TEAM_BLUE
madcowswe 72:7996aa8286ae 69 const float d = beaconpos[1].y - beaconpos[2].y;
madcowswe 72:7996aa8286ae 70 const float i = beaconpos[0].y - beaconpos[2].y;
madcowswe 72:7996aa8286ae 71 const float j = beaconpos[0].x - beaconpos[2].x;
madcowswe 72:7996aa8286ae 72 float r1 = RawReadings[SONAR2];
madcowswe 72:7996aa8286ae 73 float r2 = RawReadings[SONAR1];
madcowswe 72:7996aa8286ae 74 float r3 = RawReadings[SONAR0];
madcowswe 72:7996aa8286ae 75 #endif
madcowswe 85:b0858346d838 76 */
madcowswe 48:254b124cef02 77
madcowswe 20:70d651156779 78 printf("ranges: 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", r1, r2, r3);
madcowswe 17:6263e90bf3ba 79
madcowswe 19:4b993a9a156e 80 float y_coor = (r1*r1-r2*r2+d*d)/(2*d);
madcowswe 17:6263e90bf3ba 81 float x_coor = (r1*r1-r3*r3+i*i+j*j)/(2*j) - (i*y_coor)/j;
madcowswe 48:254b124cef02 82
madcowswe 85:b0858346d838 83 //#ifdef TEAM_RED
madcowswe 20:70d651156779 84 //coordinate system hack (for now)
madcowswe 20:70d651156779 85 x_coor = beaconpos[2].x - x_coor;
madcowswe 20:70d651156779 86 y_coor = beaconpos[2].y - y_coor;
madcowswe 85:b0858346d838 87 //#endif
madcowswe 85:b0858346d838 88 /*
madcowswe 72:7996aa8286ae 89 #ifdef TEAM_BLUE
madcowswe 72:7996aa8286ae 90 x_coor = x_coor - beaconpos[2].x;
madcowswe 72:7996aa8286ae 91 y_coor = y_coor - beaconpos[2].y;
madcowswe 72:7996aa8286ae 92 #endif
madcowswe 85:b0858346d838 93 */
madcowswe 48:254b124cef02 94
madcowswe 20:70d651156779 95 printf("solved pos from sonar: %f, %f \r\n", x_coor, y_coor);
madcowswe 48:254b124cef02 96
madcowswe 16:52250d8d8fce 97 //IR
madcowswe 16:52250d8d8fce 98 float IRMeasuresloc[3];
madcowswe 16:52250d8d8fce 99 IRMeasuresloc[0] = RawReadings[IR0];
madcowswe 16:52250d8d8fce 100 IRMeasuresloc[1] = RawReadings[IR1];
madcowswe 16:52250d8d8fce 101 IRMeasuresloc[2] = RawReadings[IR2];
madcowswe 20:70d651156779 102 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 103
madcowswe 31:ada943ecaceb 104 float IR_Offsets[3] = {0};
madcowswe 31:ada943ecaceb 105 float frombrefoffset = 0;
madcowswe 31:ada943ecaceb 106 int refbeacon = 0;
madcowswe 48:254b124cef02 107
madcowswe 48:254b124cef02 108 for (int i = 0; i < 3; i++) {
madcowswe 48:254b124cef02 109 if (sensorseenflags & 1<<(3+i)) {
madcowswe 31:ada943ecaceb 110 refbeacon = i;
madcowswe 31:ada943ecaceb 111 break;
madcowswe 31:ada943ecaceb 112 }
madcowswe 31:ada943ecaceb 113 }
madcowswe 48:254b124cef02 114
madcowswe 31:ada943ecaceb 115 printf("refbeacon is %d\r\n", refbeacon);
madcowswe 48:254b124cef02 116
madcowswe 31:ada943ecaceb 117 int cnt = 0;
madcowswe 16:52250d8d8fce 118 for (int i = 0; i < 3; i++) {
madcowswe 16:52250d8d8fce 119
madcowswe 48:254b124cef02 120 if (sensorseenflags & 1<<(3+i)) {
madcowswe 31:ada943ecaceb 121 cnt++;
madcowswe 48:254b124cef02 122
madcowswe 31:ada943ecaceb 123 //Compute IR offset
madcowswe 31:ada943ecaceb 124 float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
madcowswe 48:254b124cef02 125
madcowswe 31:ada943ecaceb 126 //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
madcowswe 31:ada943ecaceb 127 IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
madcowswe 48:254b124cef02 128
madcowswe 31:ada943ecaceb 129 frombrefoffset += constrainAngle(IR_Offsets[i] - IR_Offsets[refbeacon]);
madcowswe 31:ada943ecaceb 130 }
madcowswe 16:52250d8d8fce 131 }
madcowswe 49:665bdca0f2cd 132
madcowswe 49:665bdca0f2cd 133 printf("Used IR info from %d beacons\r\n", cnt);
madcowswe 20:70d651156779 134
madcowswe 48:254b124cef02 135 X(3,0) = constrainAngle(IR_Offsets[refbeacon] + frombrefoffset/cnt);
madcowswe 16:52250d8d8fce 136
madcowswe 16:52250d8d8fce 137 //debug
madcowswe 48:254b124cef02 138 printf("Offsets IR: %0.4f\r\n",X(3,0)*180/PI);
madcowswe 16:52250d8d8fce 139
madcowswe 16:52250d8d8fce 140 statelock.lock();
madcowswe 48:254b124cef02 141 X(0,0) = x_coor-TURRET_FWD_PLACEMENT;
madcowswe 19:4b993a9a156e 142 X(1,0) = y_coor;
madcowswe 48:254b124cef02 143 X(2,0) = 0; //TODO: assume facing east, need to account for both starting pos
madcowswe 48:254b124cef02 144
madcowswe 48:254b124cef02 145 P = 0.02*0.02, 0, 0, 0,
madcowswe 48:254b124cef02 146 0, 0.02*0.02, 0, 0,
madcowswe 79:0d3140048526 147 0, 0, 0.1*0.1, -0.1*0.1,
madcowswe 79:0d3140048526 148 0, 0, -0.1*0.1, 0.1*0.1 + 0.05*0.05;
madcowswe 48:254b124cef02 149
madcowswe 16:52250d8d8fce 150 statelock.unlock();
madcowswe 48:254b124cef02 151
madcowswe 20:70d651156779 152 Kalman_inited = 1;
madcowswe 16:52250d8d8fce 153 }
madcowswe 16:52250d8d8fce 154
madcowswe 20:70d651156779 155
madcowswe 48:254b124cef02 156 State getState()
madcowswe 48:254b124cef02 157 {
madcowswe 20:70d651156779 158 statelock.lock();
madcowswe 20:70d651156779 159 State state = {X(0,0), X(1,0), X(2,0)};
madcowswe 20:70d651156779 160 statelock.unlock();
madcowswe 20:70d651156779 161 return state;
madcowswe 20:70d651156779 162 }
madcowswe 20:70d651156779 163
madcowswe 20:70d651156779 164
madcowswe 21:167dacfe0b14 165 void predictloop(void const*)
madcowswe 16:52250d8d8fce 166 {
madcowswe 16:52250d8d8fce 167
madcowswe 49:665bdca0f2cd 168 OLED4 = !Printing::registerID(0, 3) || OLED4;
madcowswe 49:665bdca0f2cd 169 OLED4 = !Printing::registerID(1, 4) || OLED4;
madcowswe 49:665bdca0f2cd 170 OLED4 = !Printing::registerID(8, 1) || OLED4;
madcowswe 16:52250d8d8fce 171
madcowswe 16:52250d8d8fce 172 float lastleft = 0;
madcowswe 16:52250d8d8fce 173 float lastright = 0;
madcowswe 16:52250d8d8fce 174
madcowswe 16:52250d8d8fce 175 while (1) {
madcowswe 16:52250d8d8fce 176 Thread::signal_wait(0x1);
madcowswe 16:52250d8d8fce 177 OLED1 = !OLED1;
madcowswe 16:52250d8d8fce 178
madcowswe 20:70d651156779 179 float leftenc = left_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 20:70d651156779 180 float rightenc = right_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 16:52250d8d8fce 181
madcowswe 20:70d651156779 182 float dleft = leftenc-lastleft;
madcowswe 20:70d651156779 183 float dright = rightenc-lastright;
madcowswe 16:52250d8d8fce 184
madcowswe 16:52250d8d8fce 185 lastleft = leftenc;
madcowswe 16:52250d8d8fce 186 lastright = rightenc;
madcowswe 16:52250d8d8fce 187
madcowswe 16:52250d8d8fce 188
madcowswe 16:52250d8d8fce 189 //The below calculation are in body frame (where +x is forward)
madcowswe 16:52250d8d8fce 190 float dxp, dyp,d,r;
madcowswe 20:70d651156779 191 float thetap = (dright - dleft) / ENCODER_WHEELBASE;
madcowswe 20:70d651156779 192 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 193 d = (dright + dleft)/2.0f;
madcowswe 16:52250d8d8fce 194 dxp = d*cos(thetap/2.0f);
madcowswe 16:52250d8d8fce 195 dyp = d*sin(thetap/2.0f);
madcowswe 16:52250d8d8fce 196
madcowswe 16:52250d8d8fce 197 } else { //calculate circle arc
madcowswe 16:52250d8d8fce 198 //float r = (right + left) / (4.0f * PI * thetap);
madcowswe 16:52250d8d8fce 199 r = (dright + dleft) / (2.0f*thetap);
madcowswe 20:70d651156779 200 dxp = r*sin(thetap);
madcowswe 16:52250d8d8fce 201 dyp = r - r*cos(thetap);
madcowswe 16:52250d8d8fce 202 }
madcowswe 16:52250d8d8fce 203
madcowswe 16:52250d8d8fce 204 statelock.lock();
madcowswe 16:52250d8d8fce 205
madcowswe 20:70d651156779 206 float tempX2 = X(2,0);
madcowswe 16:52250d8d8fce 207 //rotating to cartesian frame and updating state
madcowswe 20:70d651156779 208 X(0,0) += dxp * cos(X(2,0)) - dyp * sin(X(2,0));
madcowswe 20:70d651156779 209 X(1,0) += dxp * sin(X(2,0)) + dyp * cos(X(2,0));
madcowswe 20:70d651156779 210 X(2,0) = constrainAngle(X(2,0) + thetap);
madcowswe 48:254b124cef02 211 //X(3,0) += 0;
madcowswe 16:52250d8d8fce 212
madcowswe 16:52250d8d8fce 213 //Linearising F around X
madcowswe 48:254b124cef02 214 Matrix<float, 4, 4> F;
madcowswe 48:254b124cef02 215 F = 1, 0, (dxp * -sin(tempX2) - dyp * cos(tempX2)), 0,
madcowswe 48:254b124cef02 216 0, 1, (dxp * cos(tempX2) - dyp * sin(tempX2)), 0,
madcowswe 48:254b124cef02 217 0, 0, 1, 0,
madcowswe 48:254b124cef02 218 0, 0, 0, 1;
madcowswe 16:52250d8d8fce 219
madcowswe 16:52250d8d8fce 220 //Generating forward and rotational variance
madcowswe 16:52250d8d8fce 221 float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f;
madcowswe 16:52250d8d8fce 222 float varang = varperang * abs(thetap);
madcowswe 20:70d651156779 223 float varxydt = xyvarpertime * KALMAN_PREDICT_PERIOD;
madcowswe 20:70d651156779 224 float varangdt = angvarpertime * KALMAN_PREDICT_PERIOD;
madcowswe 16:52250d8d8fce 225
madcowswe 16:52250d8d8fce 226 //Rotating into cartesian frame
madcowswe 16:52250d8d8fce 227 Matrix<float, 2, 2> Qsub,Qsubrot,Qrot;
madcowswe 16:52250d8d8fce 228 Qsub = varfwd + varxydt, 0,
madcowswe 16:52250d8d8fce 229 0, varxydt;
madcowswe 16:52250d8d8fce 230
madcowswe 20:70d651156779 231 Qrot = Rotmatrix(X(2,0));
madcowswe 16:52250d8d8fce 232
madcowswe 16:52250d8d8fce 233 Qsubrot = Qrot * Qsub * trans(Qrot);
madcowswe 16:52250d8d8fce 234
madcowswe 16:52250d8d8fce 235 //Generate Q
madcowswe 48:254b124cef02 236 Matrix<float, 4, 4> Q;//(Qsubrot);
madcowswe 48:254b124cef02 237 Q = Qsubrot(0,0), Qsubrot(0,1), 0, 0,
madcowswe 48:254b124cef02 238 Qsubrot(1,0), Qsubrot(1,1), 0, 0,
madcowswe 48:254b124cef02 239 0, 0, varang + varangdt, 0,
madcowswe 48:254b124cef02 240 0, 0, 0, 0;
madcowswe 16:52250d8d8fce 241
madcowswe 16:52250d8d8fce 242 P = F * P * trans(F) + Q;
madcowswe 16:52250d8d8fce 243
madcowswe 20:70d651156779 244 //printf("x: %f, y: %f, t: %f\r\n", X(0,0), X(1,0), X(2,0));
madcowswe 21:167dacfe0b14 245 //Update Printing
madcowswe 21:167dacfe0b14 246 float statecpy[] = {X(0,0), X(1,0), X(2,0)};
madcowswe 21:167dacfe0b14 247 Printing::updateval(0, statecpy, 3);
madcowswe 49:665bdca0f2cd 248
madcowswe 49:665bdca0f2cd 249 Printing::updateval(8, X(3,0));
madcowswe 16:52250d8d8fce 250
madcowswe 21:167dacfe0b14 251 float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
madcowswe 21:167dacfe0b14 252 Printing::updateval(1, Pcpy, 4);
madcowswe 16:52250d8d8fce 253
madcowswe 16:52250d8d8fce 254 statelock.unlock();
madcowswe 16:52250d8d8fce 255 }
madcowswe 16:52250d8d8fce 256 }
madcowswe 16:52250d8d8fce 257
madcowswe 20:70d651156779 258
madcowswe 48:254b124cef02 259 void predict_event_setter()
madcowswe 48:254b124cef02 260 {
madcowswe 20:70d651156779 261 if(predict_thread_ptr)
madcowswe 20:70d651156779 262 predict_thread_ptr->signal_set(0x1);
madcowswe 20:70d651156779 263 else
madcowswe 20:70d651156779 264 OLED4 = 1;
madcowswe 20:70d651156779 265 }
madcowswe 20:70d651156779 266
madcowswe 48:254b124cef02 267 void start_predict_ticker(Thread* predict_thread_ptr_in)
madcowswe 48:254b124cef02 268 {
madcowswe 20:70d651156779 269 predict_thread_ptr = predict_thread_ptr_in;
madcowswe 20:70d651156779 270 predictticker.attach(predict_event_setter, KALMAN_PREDICT_PERIOD);
madcowswe 20:70d651156779 271 }
madcowswe 20:70d651156779 272
madcowswe 20:70d651156779 273 void runupdate(measurement_t type, float value, float variance)
madcowswe 16:52250d8d8fce 274 {
madcowswe 31:ada943ecaceb 275 sensorseenflags |= 1<<type;
madcowswe 49:665bdca0f2cd 276 RawReadings[type] = value;
madcowswe 31:ada943ecaceb 277
madcowswe 48:254b124cef02 278 if (Kalman_inited) {
madcowswe 16:52250d8d8fce 279 measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
madcowswe 16:52250d8d8fce 280 if (measured) {
madcowswe 16:52250d8d8fce 281 measured->mtype = type;
madcowswe 31:ada943ecaceb 282 measured->value = RawReadings[type];
madcowswe 16:52250d8d8fce 283 measured->variance = variance;
madcowswe 16:52250d8d8fce 284
madcowswe 16:52250d8d8fce 285 osStatus putret = measureMQ.put(measured);
madcowswe 20:70d651156779 286 //if (putret)
madcowswe 48:254b124cef02 287 //OLED4 = 1;
madcowswe 16:52250d8d8fce 288 // printf("putting in MQ error code %#x\r\n", putret);
madcowswe 16:52250d8d8fce 289 } else {
madcowswe 20:70d651156779 290 //OLED4 = 1;
madcowswe 16:52250d8d8fce 291 //printf("MQalloc returned NULL ptr\r\n");
madcowswe 16:52250d8d8fce 292 }
madcowswe 48:254b124cef02 293
madcowswe 16:52250d8d8fce 294 }
madcowswe 48:254b124cef02 295
madcowswe 16:52250d8d8fce 296
madcowswe 16:52250d8d8fce 297 }
madcowswe 26:7cb3a21d9a2e 298
madcowswe 21:167dacfe0b14 299 void Kalman::updateloop(void const*)
madcowswe 16:52250d8d8fce 300 {
madcowswe 16:52250d8d8fce 301
madcowswe 16:52250d8d8fce 302 //sonar Y chanels
madcowswe 26:7cb3a21d9a2e 303 OLED4 = !Printing::registerID(2, 1);
madcowswe 26:7cb3a21d9a2e 304 OLED4 = !Printing::registerID(3, 1);
madcowswe 26:7cb3a21d9a2e 305 OLED4 = !Printing::registerID(4, 1);
madcowswe 16:52250d8d8fce 306
madcowswe 16:52250d8d8fce 307 //IR Y chanels
madcowswe 26:7cb3a21d9a2e 308 OLED4 = !Printing::registerID(5, 1);
madcowswe 26:7cb3a21d9a2e 309 OLED4 = !Printing::registerID(6, 1);
madcowswe 26:7cb3a21d9a2e 310 OLED4 = !Printing::registerID(7, 1);
madcowswe 16:52250d8d8fce 311
madcowswe 16:52250d8d8fce 312 bool aborton2stddev = false;
madcowswe 16:52250d8d8fce 313
madcowswe 48:254b124cef02 314 Matrix<float, 1, 4> H;
madcowswe 16:52250d8d8fce 315
madcowswe 26:7cb3a21d9a2e 316 float Y,S;
madcowswe 48:254b124cef02 317 const Matrix<float, 4, 4> I4( identity< Matrix<float, 4, 4> >() );
madcowswe 16:52250d8d8fce 318
madcowswe 16:52250d8d8fce 319
madcowswe 16:52250d8d8fce 320 while (1) {
madcowswe 16:52250d8d8fce 321 OLED2 = !OLED2;
madcowswe 16:52250d8d8fce 322
madcowswe 16:52250d8d8fce 323 osEvent evt = measureMQ.get();
madcowswe 16:52250d8d8fce 324
madcowswe 16:52250d8d8fce 325 if (evt.status == osEventMail) {
madcowswe 16:52250d8d8fce 326
madcowswe 16:52250d8d8fce 327 measurmentdata &measured = *(measurmentdata*)evt.value.p;
madcowswe 26:7cb3a21d9a2e 328 measurement_t type = measured.mtype; //Note, may support more measurment types than sonar in the future!
madcowswe 26:7cb3a21d9a2e 329 float value = measured.value;
madcowswe 26:7cb3a21d9a2e 330 float variance = measured.variance;
madcowswe 16:52250d8d8fce 331
madcowswe 16:52250d8d8fce 332 // don't forget to free the memory
madcowswe 16:52250d8d8fce 333 measureMQ.free(&measured);
madcowswe 16:52250d8d8fce 334
madcowswe 16:52250d8d8fce 335 if (type <= maxmeasure) {
madcowswe 16:52250d8d8fce 336
madcowswe 26:7cb3a21d9a2e 337 if (type <= SONAR2) {
madcowswe 16:52250d8d8fce 338
madcowswe 26:7cb3a21d9a2e 339 float dist = value;
madcowswe 16:52250d8d8fce 340 int sonarid = type;
madcowswe 16:52250d8d8fce 341 aborton2stddev = true;
madcowswe 16:52250d8d8fce 342
madcowswe 16:52250d8d8fce 343 statelock.lock();
madcowswe 48:254b124cef02 344
madcowswe 26:7cb3a21d9a2e 345 float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0));
madcowswe 26:7cb3a21d9a2e 346 float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0));
madcowswe 48:254b124cef02 347
madcowswe 26:7cb3a21d9a2e 348 float rbx = X(0,0) + fp_ct - beaconpos[sonarid].x;
madcowswe 26:7cb3a21d9a2e 349 float rby = X(1,0) + fp_st - beaconpos[sonarid].y;
madcowswe 48:254b124cef02 350
madcowswe 26:7cb3a21d9a2e 351 float expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
madcowswe 16:52250d8d8fce 352 Y = dist - expecdist;
madcowswe 16:52250d8d8fce 353
madcowswe 16:52250d8d8fce 354 //send to ui
madcowswe 26:7cb3a21d9a2e 355 Printing::updateval(sonarid+2, Y);
madcowswe 16:52250d8d8fce 356
madcowswe 26:7cb3a21d9a2e 357 float r_expecdist = 1.0f/expecdist;
madcowswe 16:52250d8d8fce 358
madcowswe 26:7cb3a21d9a2e 359 float dhdx = rbx * r_expecdist;
madcowswe 26:7cb3a21d9a2e 360 float dhdy = rby * r_expecdist;
madcowswe 27:664e81033846 361 float dhdt = fp_ct*dhdy - fp_st*dhdx;
madcowswe 16:52250d8d8fce 362
madcowswe 48:254b124cef02 363 H = dhdx, dhdy, dhdt, 0;
madcowswe 16:52250d8d8fce 364
madcowswe 26:7cb3a21d9a2e 365 } else if (type <= IR2) {
madcowswe 26:7cb3a21d9a2e 366
madcowswe 26:7cb3a21d9a2e 367 aborton2stddev = true;
madcowswe 16:52250d8d8fce 368 int IRidx = type-3;
madcowswe 16:52250d8d8fce 369
madcowswe 26:7cb3a21d9a2e 370 statelock.lock();
madcowswe 48:254b124cef02 371
madcowswe 26:7cb3a21d9a2e 372 float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0));
madcowswe 26:7cb3a21d9a2e 373 float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0));
madcowswe 16:52250d8d8fce 374
madcowswe 26:7cb3a21d9a2e 375 float brx = beaconpos[IRidx].x - (X(0,0) + fp_ct);
madcowswe 26:7cb3a21d9a2e 376 float bry = beaconpos[IRidx].y - (X(1,0) + fp_st);
madcowswe 16:52250d8d8fce 377
madcowswe 48:254b124cef02 378 float expecang = atan2(bry, brx) - X(2,0) + X(3,0); //constrainAngle can be called late
madcowswe 26:7cb3a21d9a2e 379 Y = constrainAngle(value - expecang);
madcowswe 16:52250d8d8fce 380
madcowswe 16:52250d8d8fce 381 //send to ui
madcowswe 26:7cb3a21d9a2e 382 Printing::updateval(IRidx + 5, Y);
madcowswe 16:52250d8d8fce 383
madcowswe 26:7cb3a21d9a2e 384 float r_dstsq = 1.0f/(brx*brx + bry*bry);
madcowswe 26:7cb3a21d9a2e 385 float dhdx = -bry*r_dstsq;
madcowswe 26:7cb3a21d9a2e 386 float dhdy = brx*r_dstsq;
madcowswe 26:7cb3a21d9a2e 387 float dhdt = fp_ct*dhdy - fp_st*dhdx - 1.0f;
madcowswe 48:254b124cef02 388 float dhdp = 1;
madcowswe 48:254b124cef02 389 H = dhdx, dhdy, dhdt, dhdp;
madcowswe 16:52250d8d8fce 390 }
madcowswe 16:52250d8d8fce 391
madcowswe 48:254b124cef02 392 Matrix<float, 4, 1> PHt (P * trans(H));
madcowswe 79:0d3140048526 393 S = (H * PHt)(0,0) + variance*4; //TODO: Temp Hack!
madcowswe 16:52250d8d8fce 394
madcowswe 48:254b124cef02 395 OLED3 = 0;
madcowswe 16:52250d8d8fce 396 if (aborton2stddev && Y*Y > 4 * S) {
madcowswe 48:254b124cef02 397 OLED3 = 1;
madcowswe 16:52250d8d8fce 398 statelock.unlock();
madcowswe 16:52250d8d8fce 399 continue;
madcowswe 16:52250d8d8fce 400 }
madcowswe 16:52250d8d8fce 401
madcowswe 48:254b124cef02 402 Matrix<float, 4, 1> K (PHt * (1/S));
madcowswe 16:52250d8d8fce 403
madcowswe 16:52250d8d8fce 404 //Updating state
madcowswe 26:7cb3a21d9a2e 405 X += K * Y;
madcowswe 26:7cb3a21d9a2e 406 X(2,0) = constrainAngle(X(2,0));
madcowswe 48:254b124cef02 407 X(3,0) = constrainAngle(X(3,0));
madcowswe 16:52250d8d8fce 408
madcowswe 48:254b124cef02 409 P = (I4 - K * H) * P;
madcowswe 16:52250d8d8fce 410
madcowswe 16:52250d8d8fce 411 statelock.unlock();
madcowswe 16:52250d8d8fce 412
madcowswe 16:52250d8d8fce 413 }
madcowswe 16:52250d8d8fce 414
madcowswe 16:52250d8d8fce 415 } else {
madcowswe 16:52250d8d8fce 416 OLED4 = 1;
madcowswe 16:52250d8d8fce 417 //printf("ERROR: in updateloop, code %#x", evt);
madcowswe 16:52250d8d8fce 418 }
madcowswe 16:52250d8d8fce 419
madcowswe 16:52250d8d8fce 420 }
madcowswe 16:52250d8d8fce 421
madcowswe 16:52250d8d8fce 422 }
madcowswe 16:52250d8d8fce 423
madcowswe 19:4b993a9a156e 424
madcowswe 16:52250d8d8fce 425 } //Kalman Namespace