Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/Kalman/Kalman.cpp@72:7996aa8286ae, 2013-04-15 (annotated)

Committer:
madcowswe
Date:
Mon Apr 15 13:37:32 2013 +0000
Revision:
72:7996aa8286ae
Parent:
49:665bdca0f2cd
Child:
79:0d3140048526
Working both sides beacons

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