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