commit!
Embed:
(wiki syntax)
Show/hide line numbers
Kalman.cpp
00001 //*************************************************************************************** 00002 //Kalman Filter implementation 00003 //*************************************************************************************** 00004 #include "Kalman.h" 00005 #include "rtos.h" 00006 #include "RFSRF05.h" 00007 #include "math.h" 00008 #include "globals.h" 00009 #include "motors.h" 00010 #include "system.h" 00011 #include "geometryfuncs.h" 00012 00013 #include <tvmet/Matrix.h> 00014 #include <tvmet/Vector.h> 00015 using namespace tvmet; 00016 00017 Kalman::Kalman(Motors &motorsin, 00018 UI &uiin, 00019 PinName Sonar_Trig, 00020 PinName Sonar_Echo0, 00021 PinName Sonar_Echo1, 00022 PinName Sonar_Echo2, 00023 PinName Sonar_Echo3, 00024 PinName Sonar_Echo4, 00025 PinName Sonar_Echo5, 00026 PinName Sonar_SDI, 00027 PinName Sonar_SDO, 00028 PinName Sonar_SCK, 00029 PinName Sonar_NCS, 00030 PinName Sonar_NIRQ) : 00031 ir(*this), 00032 sonararray(Sonar_Trig, 00033 Sonar_Echo0, 00034 Sonar_Echo1, 00035 Sonar_Echo2, 00036 Sonar_Echo3, 00037 Sonar_Echo4, 00038 Sonar_Echo5, 00039 Sonar_SDI, 00040 Sonar_SDO, 00041 Sonar_SCK, 00042 Sonar_NCS, 00043 Sonar_NIRQ), 00044 motors(motorsin), 00045 ui(uiin), 00046 predictthread(predictloopwrapper, this, osPriorityNormal, 512), 00047 predictticker( SIGTICKARGS(predictthread, 0x1) ), 00048 // sonarthread(sonarloopwrapper, this, osPriorityNormal, 256), 00049 // sonarticker( SIGTICKARGS(sonarthread, 0x1) ), 00050 updatethread(updateloopwrapper, this, osPriorityNormal, 512) { 00051 00052 //Initilising offsets 00053 InitLock.lock(); 00054 IR_Offset = 0; 00055 Sonar_Offset = 0; 00056 InitLock.unlock(); 00057 00058 00059 //Initilising matrices 00060 00061 // X = x, y, theta; 00062 if (Colour) 00063 X = 0.5, 0, 0; 00064 else 00065 X = 2.5, 0, PI; 00066 00067 P = 1, 0, 0, 00068 0, 1, 0, 00069 0, 0, 0.04; 00070 00071 //measurment variance R is provided by each sensor when calling runupdate 00072 00073 //attach callback 00074 sonararray.callbackobj = (DummyCT*)this; 00075 sonararray.mcallbackfunc = (void (DummyCT::*)(int beaconnum, float distance, float variance)) &Kalman::runupdate; 00076 00077 00078 predictticker.start(20); 00079 // sonarticker.start(50); 00080 00081 } 00082 00083 00084 //Note: this init function assumes that the robot faces east, theta=0, in the +x direction 00085 void Kalman::KalmanInit() { 00086 motors.stop(); 00087 float SonarMeasuresx1000[3]; 00088 float IRMeasuresloc[3]; 00089 int beacon_cnt = 0; 00090 00091 00092 // doesn't work since they break the ISR 00093 /* 00094 #ifdef ROBOT_PRIMARY 00095 LPC_UART3->FCR = LPC_UART3->FCR | 0x06; // Flush the serial FIFO buffer / OR with FCR 00096 #else 00097 LPC_UART1->FCR = LPC_UART1->FCR | 0x06; // Flush the serial FIFO buffer / OR with FCR 00098 #endif 00099 */ 00100 // zeros the measurements 00101 for (int i = 0; i < 3; i++) { 00102 SonarMeasures[i] = 0; 00103 IRMeasures[i] = 0; 00104 } 00105 00106 InitLock.lock(); 00107 //zeros offsets 00108 IR_Offset = 0; 00109 Sonar_Offset = 0; 00110 InitLock.unlock(); 00111 00112 // attaches ir interrup 00113 ir.attachisr(); 00114 00115 //wating untill the IR has reved up and picked up some valid data 00116 //Thread::wait(1000); 00117 wait(2); 00118 00119 //temporaraly disable IR updates 00120 ir.detachisr(); 00121 00122 //lock the state throughout the computation, as we will override the state at the end 00123 InitLock.lock(); 00124 statelock.lock(); 00125 00126 00127 00128 SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f; 00129 SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f; 00130 SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f; 00131 IRMeasuresloc[0] = IRMeasures[0]; 00132 IRMeasuresloc[1] = IRMeasures[1]; 00133 IRMeasuresloc[2] = IRMeasures[2]; 00134 //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI); 00135 00136 float d = beaconpos[2].y - beaconpos[1].y; 00137 float i = beaconpos[0].y - beaconpos[1].y; 00138 float j = beaconpos[0].x - beaconpos[1].x; 00139 float origin_x = beaconpos[1].x; 00140 float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d); 00141 float x_coor = origin_x + (SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j; 00142 00143 //debug for trilateration 00144 printf("Cal at x: %0.4f, y: %0.4f \r\n",x_coor,y_coor ); 00145 00146 float Dist_Exp[3]; 00147 for (int i = 0; i < 3; i++) { 00148 //Compute sonar offset 00149 Dist_Exp[i] = hypot(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor); 00150 Sonar_Offset += (SonarMeasuresx1000[i]-Dist_Exp[i])/3000.0f; 00151 00152 //Compute IR offset 00153 float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor); 00154 if (!Colour) 00155 angle_est -= PI; 00156 //printf("Angle %d : %f \n\r",i,angle_est*180/PI ); 00157 // take average offset angle from valid readings 00158 if (IRMeasuresloc[i] != 0) { 00159 beacon_cnt ++; 00160 // changed to current angle - estimated angle 00161 float angle_temp = IRMeasuresloc[i] - angle_est; 00162 angle_temp -= (floor(angle_temp/(2*PI)))*2*PI; 00163 IR_Offset += angle_temp; 00164 } 00165 } 00166 IR_Offset /= float(beacon_cnt); 00167 00168 //debug 00169 printf("Offsets IR: %0.4f, Sonar: %0.4f \r\n",IR_Offset*180/PI,Sonar_Offset*1000 ); 00170 00171 //statelock already locked 00172 X(0) = x_coor/1000.0f; 00173 X(1) = y_coor/1000.0f; 00174 00175 if (Colour) 00176 X(2) = 0; 00177 else 00178 X(2) = PI; 00179 00180 // unlocks mutexes 00181 InitLock.unlock(); 00182 statelock.unlock(); 00183 00184 00185 //reattach the IR processing 00186 ir.attachisr(); 00187 } 00188 00189 00190 void Kalman::predictloop() { 00191 00192 OLED4 = !ui.regid(0, 3); 00193 OLED4 = !ui.regid(1, 4); 00194 00195 float lastleft = 0; 00196 float lastright = 0; 00197 00198 while (1) { 00199 Thread::signal_wait(0x1); 00200 OLED1 = !OLED1; 00201 00202 int leftenc = motors.getEncoder1(); 00203 int rightenc = motors.getEncoder2(); 00204 00205 float dleft = motors.encoderToDistance(leftenc-lastleft)/1000.0f; 00206 float dright = motors.encoderToDistance(rightenc-lastright)/1000.0f; 00207 00208 lastleft = leftenc; 00209 lastright = rightenc; 00210 00211 00212 //The below calculation are in body frame (where +x is forward) 00213 float dxp, dyp,d,r; 00214 float thetap = (dright - dleft)*PI / (float(robotCircumference)/1000.0f); 00215 if (abs(thetap) < 0.02) { //if the rotation through the integration step is small, approximate with a straight line to avoid numerical error 00216 d = (dright + dleft)/2.0f; 00217 dxp = d*cos(thetap/2.0f); 00218 dyp = d*sin(thetap/2.0f); 00219 00220 } else { //calculate circle arc 00221 //float r = (right + left) / (4.0f * PI * thetap); 00222 r = (dright + dleft) / (2.0f*thetap); 00223 dxp = abs(r)*sin(thetap); 00224 dyp = r - r*cos(thetap); 00225 } 00226 00227 statelock.lock(); 00228 00229 float tempX2 = X(2); 00230 //rotating to cartesian frame and updating state 00231 X(0) += dxp * cos(X(2)) - dyp * sin(X(2)); 00232 X(1) += dxp * sin(X(2)) + dyp * cos(X(2)); 00233 X(2) = rectifyAng(X(2) + thetap); 00234 00235 //Linearising F around X 00236 float avgX2 = (X(2) + tempX2)/2.0f; 00237 Matrix<float, 3, 3> F; 00238 F = 1, 0, (dxp * -sin(avgX2) - dyp * cos(avgX2)), 00239 0, 1, (dxp * cos(avgX2) - dyp * sin(avgX2)), 00240 0, 0, 1; 00241 00242 //Generating forward and rotational variance 00243 float varfwd = fwdvarperunit * abs(dright + dleft) / 2.0f; 00244 float varang = varperang * abs(thetap); 00245 float varxydt = xyvarpertime * PREDICTPERIOD/1000.0f; 00246 float varangdt = angvarpertime * PREDICTPERIOD/1000.0f; 00247 00248 //Rotating into cartesian frame 00249 Matrix<float, 2, 2> Qsub,Qsubrot,Qrot; 00250 Qsub = varfwd + varxydt, 0, 00251 0, varxydt; 00252 00253 Qrot = Rotmatrix(X(2)); 00254 00255 Qsubrot = Qrot * Qsub * trans(Qrot); 00256 00257 //Generate Q 00258 Matrix<float, 3, 3> Q;//(Qsubrot); 00259 Q = Qsubrot(0,0), Qsubrot(0,1), 0, 00260 Qsubrot(1,0), Qsubrot(1,1), 0, 00261 0, 0, varang + varangdt; 00262 00263 P = F * P * trans(F) + Q; 00264 00265 //Update UI 00266 float statecpy[] = {X(0), X(1), X(2)}; 00267 ui.updateval(0, statecpy, 3); 00268 00269 float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)}; 00270 ui.updateval(1, Pcpy, 4); 00271 00272 statelock.unlock(); 00273 } 00274 } 00275 00276 //void Kalman::sonarloop() { 00277 // while (1) { 00278 // Thread::signal_wait(0x1); 00279 // sonararray.startRange(); 00280 // } 00281 //} 00282 00283 00284 void Kalman::runupdate(measurement_t type, float value, float variance) { 00285 //printf("beacon %d dist %f\r\n", sonarid, dist); 00286 //led2 = !led2; 00287 00288 measurmentdata* measured = (measurmentdata*)measureMQ.alloc(); 00289 if (measured) { 00290 measured->mtype = type; 00291 measured->value = value; 00292 measured->variance = variance; 00293 00294 osStatus putret = measureMQ.put(measured); 00295 if (putret) 00296 OLED4 = 1; 00297 // printf("putting in MQ error code %#x\r\n", putret); 00298 } else { 00299 OLED4 = 1; 00300 //printf("MQalloc returned NULL ptr\r\n"); 00301 } 00302 00303 } 00304 00305 void Kalman::updateloop() { 00306 00307 //sonar Y chanels 00308 ui.regid(2, 1); 00309 ui.regid(3, 1); 00310 ui.regid(4, 1); 00311 00312 //IR Y chanels 00313 ui.regid(5, 1); 00314 ui.regid(6, 1); 00315 ui.regid(7, 1); 00316 00317 measurement_t type; 00318 float value,variance,rbx,rby,expecdist,Y; 00319 float dhdx,dhdy; 00320 bool aborton2stddev = false; 00321 00322 Matrix<float, 1, 3> H; 00323 00324 float S; 00325 Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() ); 00326 00327 00328 while (1) { 00329 OLED2 = !OLED2; 00330 00331 osEvent evt = measureMQ.get(); 00332 00333 if (evt.status == osEventMail) { 00334 00335 measurmentdata &measured = *(measurmentdata*)evt.value.p; 00336 type = measured.mtype; //Note, may support more measurment types than sonar in the future! 00337 value = measured.value; 00338 variance = measured.variance; 00339 00340 // don't forget to free the memory 00341 measureMQ.free(&measured); 00342 00343 if (type <= maxmeasure) { 00344 00345 if (type <= SONAR3) { 00346 00347 InitLock.lock(); 00348 float dist = value / 1000.0f - Sonar_Offset; //converting to m from mm,subtract the offset 00349 InitLock.unlock(); 00350 00351 int sonarid = type; 00352 aborton2stddev = true; 00353 00354 statelock.lock(); 00355 //update the current sonar readings 00356 SonarMeasures[sonarid] = dist; 00357 00358 rbx = X(0) - beaconpos[sonarid].x/1000.0f; 00359 rby = X(1) - beaconpos[sonarid].y/1000.0f; 00360 00361 expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby); 00362 Y = dist - expecdist; 00363 00364 //send to ui 00365 ui.updateval(sonarid+2, Y); 00366 00367 dhdx = rbx / expecdist; 00368 dhdy = rby / expecdist; 00369 00370 H = dhdx, dhdy, 0; 00371 00372 } else if (type <= IR3) { 00373 00374 aborton2stddev = false; 00375 int IRidx = type-3; 00376 00377 // subtract the IR offset 00378 InitLock.lock(); 00379 value -= IR_Offset; 00380 InitLock.unlock(); 00381 00382 statelock.lock(); 00383 IRMeasures[IRidx] = value; 00384 00385 rbx = X(0) - beaconpos[IRidx].x/1000.0f; 00386 rby = X(1) - beaconpos[IRidx].y/1000.0f; 00387 00388 float expecang = atan2(-rby, -rbx) - X(2); 00389 Y = rectifyAng(value - expecang); 00390 00391 //send to ui 00392 ui.updateval(IRidx + 5, Y); 00393 00394 float dstsq = rbx*rbx + rby*rby; 00395 H = -rby/dstsq, rbx/dstsq, -1; 00396 } 00397 00398 Matrix<float, 3, 1> PH (P * trans(H)); 00399 S = (H * PH)(0,0) + variance; 00400 00401 if (aborton2stddev && Y*Y > 4 * S) { 00402 statelock.unlock(); 00403 continue; 00404 } 00405 00406 Matrix<float, 3, 1> K (PH * (1/S)); 00407 00408 //Updating state 00409 X += col(K, 0) * Y; 00410 X(2) = rectifyAng(X(2)); 00411 00412 P = (I3 - K * H) * P; 00413 00414 statelock.unlock(); 00415 00416 } 00417 00418 } else { 00419 OLED4 = 1; 00420 //printf("ERROR: in updateloop, code %#x", evt); 00421 } 00422 00423 } 00424 00425 } 00426 00427 // reset kalman states 00428 void Kalman::KalmanReset() { 00429 float SonarMeasuresx1000[3]; 00430 statelock.lock(); 00431 SonarMeasuresx1000[0] = SonarMeasures[0]*1000.0f; 00432 SonarMeasuresx1000[1] = SonarMeasures[1]*1000.0f; 00433 SonarMeasuresx1000[2] = SonarMeasures[2]*1000.0f; 00434 //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI); 00435 00436 float d = beaconpos[2].y - beaconpos[1].y; 00437 float i = beaconpos[0].y - beaconpos[1].y; 00438 float j = beaconpos[0].x - beaconpos[1].x; 00439 float origin_x = beaconpos[1].x; 00440 float y_coor = (SonarMeasuresx1000[1]*SonarMeasuresx1000[1]- SonarMeasuresx1000[2]*SonarMeasuresx1000[2] + d*d) / (2*d); 00441 float x_coor = origin_x +(SonarMeasuresx1000[1]*SonarMeasuresx1000[1] - SonarMeasuresx1000[0]*SonarMeasuresx1000[0] + i*i + j*j)/(2*j) - i*y_coor/j; 00442 00443 //statelock already locked 00444 X(0) = x_coor/1000.0f; 00445 X(1) = y_coor/1000.0f; 00446 00447 00448 00449 /* if (Colour){ 00450 X(0) = 0.2; 00451 X(1) = 0.2; 00452 //X(2) = 0; 00453 } 00454 else { 00455 X(0) = 2.8; 00456 X(1) = 0.2; 00457 //X(2) = PI; 00458 } 00459 */ 00460 P = 0.05, 0, 0, 00461 0, 0.05, 0, 00462 0, 0, 0.04; 00463 00464 // unlocks mutexes 00465 statelock.unlock(); 00466 00467 }
Generated on Wed Jul 13 2022 17:39:34 by 1.7.2