2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Processes/Kalman/Kalman.cpp@85:b0858346d838, 2013-04-16 (annotated)
- Committer:
- madcowswe
- Date:
- Tue Apr 16 10:43:15 2013 +0000
- Revision:
- 85:b0858346d838
- Parent:
- 79:0d3140048526
diff
Who changed what in which revision?
User | Revision | Line number | New 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 |