MiniExplorerCoimbra.cpp

00001 #include "MiniExplorerCoimbra.hpp"
00002 #include "robot.h"
00003 
00004 #define PI 3.14159
00005 
00006 MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld):map(200,200,20,20),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){
00007     i2c1.frequency(100000);
00008     initRobot(); //Initializing the robot
00009     pc.baud(9600); // baud for the pc communication
00010 
00011     measure_always_on();//TODO check if needed
00012 
00013     this->setXYThetaAndXYThetaWorld(defaultXWorld,defaultYWorld,defaultThetaWorld);
00014     this->radiusWheels=3.25;
00015     this->distanceWheels=7.2; 
00016     
00017     this->khro=12;
00018     this->ka=30;
00019     this->kb=-13;
00020     this->kv=200;
00021     this->kh=200;
00022 
00023     this->rangeForce=30;
00024     this->attractionConstantForce=10000;
00025     this->repulsionConstantForce=1;
00026 } 
00027 
00028 void MiniExplorerCoimbra::setXYThetaAndXYThetaWorld(float defaultXWorld, float defaultYWorld, float defaultThetaWorld){
00029     this->xWorld=defaultXWorld;
00030     this->yWorld=defaultYWorld;
00031     this->thetaWorld=defaultThetaWorld;
00032     X=defaultYWorld;
00033     Y=-defaultXWorld;
00034     if(defaultThetaWorld < -PI/2)
00035         theta=PI/2+PI-defaultThetaWorld;
00036     else
00037         theta=defaultThetaWorld-PI/2;
00038 }
00039 
00040 void MiniExplorerCoimbra::myOdometria(){
00041     Odometria();
00042     this->xWorld=-Y;
00043     this->yWorld=X;
00044     if(theta >PI/2)
00045         this->thetaWorld=-PI+(theta-PI/2);
00046     else
00047         this->thetaWorld=theta+PI/2;
00048 }
00049 
00050 //generate a position randomly and makes the robot go there while updating the map
00051 void MiniExplorerCoimbra::randomize_and_map() {
00052     //TODO check that it's aurelien's work
00053     float movementOnX=rand()%(int)(this->map.widthRealMap/2);
00054     float movementOnY=rand()%(int)(this->map.heightRealMap/2);
00055     
00056     float signOfX=rand()%2;
00057     if(signOfX < 1)
00058         signOfX=-1;
00059     float signOfY=rand()%2;
00060     if(signOfY  < 1)
00061         signOfY=-1;
00062         
00063     float targetXWorld = movementOnX*signOfX;
00064     float targetYWorld = movementOnY*signOfY;
00065     float targetAngleWorld = 2*((float)(rand()%31416)-15708)/10000.0;
00066     this->go_to_point_with_angle(targetXWorld, targetYWorld, targetAngleWorld);
00067 }
00068 
00069 //move of targetXWorld and targetYWorld ending in a targetAngleWorld
00070 void MiniExplorerCoimbra::go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld) {
00071     bool keepGoing=true;
00072     float leftMm;
00073     float frontMm; 
00074     float rightMm;
00075     float dt;
00076     Timer t;
00077     float distanceToTarget;
00078     do {
00079         //Timer stuff
00080         dt = t.read();
00081         t.reset();
00082         t.start();
00083         
00084         //Updating X,Y and theta with the odometry values
00085         this->myOdometria();
00086         leftMm = get_distance_left_sensor();
00087         frontMm = get_distance_front_sensor();
00088         rightMm = get_distance_right_sensor();
00089 
00090         //if in dangerzone 
00091         if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){
00092             leftMotor(1,0);
00093             rightMotor(1,0);
00094             this->update_sonar_values(leftMm, frontMm, rightMm);
00095             //TODO Giorgos maybe you can also test the do_half_flip() function
00096             this->myOdometria();
00097             //do a flip TODO
00098             keepGoing=false;
00099             this->do_half_flip();   
00100         }else{
00101             //if not in danger zone continue as usual
00102             this->update_sonar_values(leftMm, frontMm, rightMm);
00103 
00104             //Updating motor velocities
00105             distanceToTarget=this->update_angular_speed_wheels_go_to_point_with_angle(targetXWorld,targetYWorld,targetAngleWorld,dt);
00106     
00107             wait(0.2);
00108             //Timer stuff
00109             t.stop();
00110             pc.printf("\n\r dist to target= %f",distanceToTarget);
00111         }
00112     } while(distanceToTarget>1 && (abs(targetAngleWorld-this->thetaWorld)>0.01) && keepGoing);
00113 
00114     //Stop at the end
00115     leftMotor(1,0);
00116     rightMotor(1,0);
00117 }
00118 
00119 float MiniExplorerCoimbra::update_angular_speed_wheels_go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld, float dt){
00120     //compute_angles_and_distance
00121     //atan2 take the deplacement on x and the deplacement on y as parameters
00122     float angleToPoint = atan2((targetYWorld-this->yWorld),(targetXWorld-this->xWorld))-this->thetaWorld;
00123     angleToPoint = atan(sin(angleToPoint)/cos(angleToPoint));
00124     //rho is the distance to the point of arrival
00125     float rho = dist(targetXWorld,targetYWorld,this->xWorld,this->yWorld);
00126     float distanceToTarget = rho;
00127     //TODO check that
00128     float beta = targetAngleWorld-angleToPoint-this->thetaWorld;        
00129     
00130     //Computing angle error and distance towards the target value
00131     rho += dt*(-this->khro*cos(angleToPoint)*rho);
00132     float temp = angleToPoint;
00133     angleToPoint += dt*(this->khro*sin(angleToPoint)-this->ka*angleToPoint-this->kb*beta);
00134     beta += dt*(-this->khro*sin(temp));
00135 
00136     //Computing linear and angular velocities
00137     float linear;
00138     float angular;
00139     if(angleToPoint>=-1.5708 && angleToPoint<=1.5708){
00140         linear=this->khro*rho;
00141         angular=this->ka*angleToPoint+this->kb*beta;
00142     }
00143     else{
00144         linear=-this->khro*rho;
00145         angular=-this->ka*angleToPoint-this->kb*beta;
00146     }
00147     float angular_left=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
00148     float angular_right=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
00149     
00150     //Normalize speed for motors
00151     if(angular_left>angular_right) {
00152         angular_right=this->speed*angular_right/angular_left;
00153         angular_left=this->speed;
00154     } else {
00155         angular_left=this->speed*angular_left/angular_right;
00156         angular_right=this->speed;
00157     }
00158 
00159     //compute_linear_angular_velocities 
00160     leftMotor(1,angular_left);
00161     rightMotor(1,angular_right);
00162     
00163     return distanceToTarget;
00164 }
00165 
00166 void MiniExplorerCoimbra::update_sonar_values(float leftMm,float frontMm,float rightMm){
00167     float xWorldCell;
00168     float yWorldCell;
00169     for(int i=0;i<this->map.nbCellWidth;i++){
00170         for(int j=0;j<this->map.nbCellHeight;j++){
00171             xWorldCell=this->map.cell_width_coordinate_to_world(i);
00172             yWorldCell=this->map.cell_height_coordinate_to_world(j);
00173             this->map.update_cell_value(i,j,this->sonarRight.compute_probability_t(rightMm/10,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
00174             this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm/10,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
00175             this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm/10,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
00176 
00177         }
00178     }
00179 }
00180 
00181 //Distance computation function
00182 float MiniExplorerCoimbra::dist(float x1, float y1, float x2, float y2){
00183     return sqrt(pow(y2-y1,2) + pow(x2-x1,2));
00184 }
00185 
00186 //use virtual force field
00187 void MiniExplorerCoimbra::try_to_reach_target(float targetXWorld,float targetYWorld){
00188     //atan2 gives the angle beetween PI and -PI
00189     this->myOdometria();
00190     /*
00191     float deplacementOnXWorld=targetXWorld-this->xWorld;
00192     float deplacementOnYWorld=targetYWorld-this->yWorld;
00193     */
00194     float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
00195     turn_to_target(angleToTarget);
00196     bool reached=false;
00197     int print=0;
00198     while (!reached) {
00199         vff(&reached,targetXWorld,targetYWorld);
00200         //test_got_to_line(&reached);
00201         if(print==10){
00202             leftMotor(1,0);
00203             rightMotor(1,0);
00204             this->print_map_with_robot_position_and_target(targetXWorld,targetYWorld);
00205             print=0;
00206         }else
00207             print+=1;
00208     }
00209     //Stop at the end
00210     leftMotor(1,0);
00211     rightMotor(1,0);
00212     pc.printf("\r\n target reached");
00213     wait(3);//
00214 }
00215 
00216 void MiniExplorerCoimbra::vff(bool* reached, float targetXWorld, float targetYWorld){
00217     float line_a;
00218     float line_b;
00219     float line_c;
00220     //Updating X,Y and theta with the odometry values
00221     float forceXWorld=0;
00222     float forceYWorld=0;
00223     //we update the odometrie
00224     this->myOdometria();
00225     //we check the sensors
00226     float leftMm = get_distance_left_sensor();
00227     float frontMm = get_distance_front_sensor();
00228     float rightMm = get_distance_right_sensor();
00229     //update the probabilities values 
00230     this->update_sonar_values(leftMm, frontMm, rightMm);
00231     //we compute the force on X and Y
00232     this->compute_forceX_and_forceY(&forceXWorld, &forceYWorld,targetXWorld,targetYWorld);
00233     //we compute a new ine
00234     this->calculate_line(forceXWorld, forceYWorld, &line_a,&line_b,&line_c);
00235     //Updating motor velocities
00236     this->go_to_line(line_a,line_b,line_c,targetXWorld,targetYWorld);
00237 
00238     //wait(0.1);
00239     this->myOdometria();
00240     if(dist(this->xWorld,this->yWorld,targetXWorld,targetYWorld)<10)
00241         *reached=true;
00242 }
00243 
00244 //compute the force on X and Y
00245 void MiniExplorerCoimbra::compute_forceX_and_forceY(float* forceXWorld, float* forceYWorld, float targetXWorld, float targetYWorld){
00246      float forceRepulsionComputedX=0;
00247      float forceRepulsionComputedY=0;
00248      //for each cell of the map we compute a force of repulsion
00249      for(int i=0;i<this->map.nbCellWidth;i++){
00250         for(int j=0;j<this->map.nbCellHeight;j++){
00251             this->update_force(i,j,&forceRepulsionComputedX,&forceRepulsionComputedY);
00252         }
00253     }
00254     //update with attraction force
00255     *forceXWorld=+forceRepulsionComputedX;
00256     *forceYWorld=+forceRepulsionComputedY;
00257     float distanceTargetRobot=sqrt(pow(targetXWorld-this->xWorld,2)+pow(targetYWorld-this->yWorld,2));
00258     if(distanceTargetRobot != 0){
00259         *forceXWorld-=this->attractionConstantForce*(targetXWorld-this->xWorld)/distanceTargetRobot;
00260         *forceYWorld-=this->attractionConstantForce*(targetYWorld-this->yWorld)/distanceTargetRobot;
00261     }
00262     float amplitude=sqrt(pow(*forceXWorld,2)+pow(*forceYWorld,2));
00263     if(amplitude!=0){//avoid division by 0 if forceX and forceY  == 0
00264         *forceXWorld=*forceXWorld/amplitude;
00265         *forceYWorld=*forceYWorld/amplitude;
00266     }
00267 }
00268 
00269 void MiniExplorerCoimbra::update_force(int widthIndice, int heightIndice, float* forceRepulsionComputedX, float* forceRepulsionComputedY ){
00270     //get the coordonate of the map and the robot in the ortonormal space
00271     float xWorldCell=this->map.cell_width_coordinate_to_world(widthIndice);
00272     float yWorldCell=this->map.cell_height_coordinate_to_world(heightIndice);
00273     //compute the distance beetween the cell and the robot
00274     float distanceCellToRobot=sqrt(pow(xWorldCell-this->xWorld,2)+pow(yWorldCell-this->yWorld,2));
00275     //check if the cell is in range
00276     if(distanceCellToRobot <= this->rangeForce) {
00277         float probaCell=this->map.get_proba_cell(widthIndice,heightIndice);
00278         *forceRepulsionComputedX+=this->repulsionConstantForce*probaCell*(xWorldCell-this->xWorld)/pow(distanceCellToRobot,3);
00279         *forceRepulsionComputedY+=this->repulsionConstantForce*probaCell*(yWorldCell-this->yWorld)/pow(distanceCellToRobot,3);
00280     }
00281 }
00282 
00283 //robotX and robotY are from this->myOdometria(), calculate line_a, line_b and line_c
00284 void MiniExplorerCoimbra::calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c){
00285     /*
00286     in the teachers maths it is 
00287     
00288     *line_a=forceY;
00289     *line_b=-forceX;
00290     
00291     because a*x+b*y+c=0
00292     a impact the vertical and b the horizontal
00293     and he has to put them like this because
00294     Robot space:      World space:
00295       ^                 ^
00296       |x                |y
00297    <- R                 O ->
00298     y                     x
00299     but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to 
00300     */
00301     *line_a=forceX;
00302     *line_b=forceY;
00303     //because the line computed always pass by the robot center we dont need lince_c
00304     //*line_c=forceX*this->yWorld+forceY*this->xWorld;    
00305     *line_c=0;
00306 }
00307 
00308 //currently line_c is not used
00309 void MiniExplorerCoimbra::go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld){
00310     float lineAngle;
00311     float angleError;
00312     float linear;
00313     float angular; 
00314     
00315     //atan2 use the deplacement on X and the deplacement on Y
00316     float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
00317     bool aligned=false;
00318     
00319     //this condition is passed if the target is in the same direction as the robot orientation
00320     if((angleToTarget>0 && this->thetaWorld > 0) || (angleToTarget<0 && this->thetaWorld <0))
00321         aligned=true;
00322     
00323     //line angle is beetween pi/2 and -pi/2
00324     /*
00325     ax+by+c=0 (here c==0)
00326     y=x*-a/b
00327     if a*b > 0, the robot is going down
00328     if a*b <0, the robot is going up
00329     */  
00330      if(line_b!=0){
00331         if(!aligned)
00332             lineAngle=atan(-line_a/line_b);
00333         else
00334             lineAngle=atan(line_a/-line_b);
00335     }
00336     else{
00337         lineAngle=1.5708;
00338     }
00339     
00340     //Computing angle error
00341     angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
00342     angleError = atan(sin(angleError)/cos(angleError));
00343 
00344     //Calculating velocities
00345     linear=this->kv*(3.1416);
00346     angular=this->kh*angleError;
00347 
00348     float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
00349     float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
00350     
00351     //Normalize speed for motors
00352     if(abs(angularLeft)>abs(angularRight)) {  
00353         angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
00354         angularLeft=this->speed*this->sign1(angularLeft);
00355     }
00356     else {
00357         angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
00358         angularRight=this->speed*this->sign1(angularRight);
00359     }
00360     leftMotor(this->sign2(angularLeft),abs(angularLeft));
00361     rightMotor(this->sign2(angularRight),abs(angularRight));
00362 }
00363 
00364 /*angleToTarget is obtained through atan2 so it s:
00365 < 0 if the angle is bettween PI and 2pi on a trigo circle
00366 > 0 if it is between 0 and PI
00367 */
00368 void MiniExplorerCoimbra::turn_to_target(float angleToTarget){
00369     this->myOdometria();
00370     float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
00371     if(theta_plus_h_pi > PI)
00372         theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
00373      if(angleToTarget>0){   
00374         leftMotor(0,1);
00375         rightMotor(1,1);
00376     }else{
00377         leftMotor(1,1);
00378         rightMotor(0,1);
00379     }
00380     while(abs(angleToTarget-theta_plus_h_pi)>0.05){
00381         this->myOdometria();
00382         theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
00383          if(theta_plus_h_pi > PI)
00384             theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
00385         //pc.printf("\n\r diff=%f", abs(angleToTarget-theta_plus_h_pi));
00386     }
00387     leftMotor(1,0);
00388     rightMotor(1,0);    
00389 }
00390 
00391 void MiniExplorerCoimbra::do_half_flip(){
00392     this->myOdometria();
00393     float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
00394     if(theta_plus_h_pi > PI)
00395         theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
00396     leftMotor(0,100);
00397     rightMotor(1,100);
00398     while(abs(theta_plus_h_pi-theta)>0.05){
00399         this->myOdometria();
00400        // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
00401     }
00402     leftMotor(1,0);
00403     rightMotor(1,0);    
00404 }
00405 
00406 void MiniExplorerCoimbra::print_map_with_robot_position() {
00407     float currProba;
00408     
00409     float heightIndiceInOrthonormal;
00410     float widthIndiceInOrthonormal;
00411     
00412     float widthMalus=-(3*this->map.sizeCellWidth/2);
00413     float widthBonus=this->map.sizeCellWidth/2;
00414     
00415     float heightMalus=-(3*this->map.sizeCellHeight/2);
00416     float heightBonus=this->map.sizeCellHeight/2;
00417 
00418     pc.printf("\n\r");
00419     for (int y = this->map.nbCellHeight -1; y>-1; y--) {
00420         for (int x= 0; x<this->map.nbCellWidth; x++) {
00421             heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
00422             widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
00423             if(this->yWorld >= (heightIndiceInOrthonormal+heightMalus) && this->yWorld <= (heightIndiceInOrthonormal+heightBonus) && this->xWorld >= (widthIndiceInOrthonormal+widthMalus) && this->xWorld <= (widthIndiceInOrthonormal+widthBonus))                    
00424                 pc.printf(" R ");
00425             else{
00426                 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
00427                 if ( currProba < 0.5)
00428                     pc.printf("   ");
00429                 else{
00430                     if(currProba==0.5)
00431                         pc.printf(" . ");
00432                     else
00433                         pc.printf(" X ");
00434                 } 
00435             }
00436         }
00437         pc.printf("\n\r");
00438     }
00439 }
00440 
00441 void MiniExplorerCoimbra::print_map_with_robot_position_and_target(float targetXWorld, float targetYWorld) {
00442     float currProba;
00443     
00444     float heightIndiceInOrthonormal;
00445     float widthIndiceInOrthonormal;
00446     
00447     float widthMalus=-(3*this->map.sizeCellWidth/2);
00448     float widthBonus=this->map.sizeCellWidth/2;
00449     
00450     float heightMalus=-(3*this->map.sizeCellHeight/2);
00451     float heightBonus=this->map.sizeCellHeight/2;
00452 
00453     pc.printf("\n\r");
00454     for (int y = this->map.nbCellHeight -1; y>-1; y--) {
00455         for (int x= 0; x<this->map.nbCellWidth; x++) {
00456             heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
00457             widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
00458             if(this->yWorld >= (heightIndiceInOrthonormal+heightMalus) && this->yWorld <= (heightIndiceInOrthonormal+heightBonus) && this->xWorld >= (widthIndiceInOrthonormal+widthMalus) && this->xWorld <= (widthIndiceInOrthonormal+widthBonus))
00459                 pc.printf(" R ");
00460             else{
00461                 if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus))                    
00462                     pc.printf(" T ");
00463                 else{
00464                     currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
00465                     if ( currProba < 0.5)
00466                         pc.printf("   ");
00467                     else{
00468                         if(currProba==0.5)
00469                             pc.printf(" . ");
00470                         else
00471                             pc.printf(" X ");
00472                     } 
00473                 }
00474             }
00475         }
00476         pc.printf("\n\r");
00477     }
00478 }
00479 
00480 void MiniExplorerCoimbra::print_map() {
00481     float currProba;
00482     pc.printf("\n\r");
00483     for (int y = this->map.nbCellHeight -1; y>-1; y--) {
00484         for (int x= 0; x<this->map.nbCellWidth; x++) {
00485                 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
00486             if ( currProba < 0.5) {
00487                 pc.printf("   ");
00488             } else {
00489                 if(currProba==0.5)
00490                     pc.printf(" . ");
00491                 else
00492                     pc.printf(" X ");
00493             }
00494         }
00495         pc.printf("\n\r");
00496     }
00497 }
00498 
00499 //return 1 if positiv, -1 if negativ
00500 float MiniExplorerCoimbra::sign1(float value){
00501     if(value>=0) 
00502         return 1;
00503     else 
00504         return -1;
00505 }
00506 
00507 //return 1 if positiv, 0 if negativ
00508 int MiniExplorerCoimbra::sign2(float value){
00509     if(value>=0) 
00510         return 1;
00511     else 
00512         return 0;
00513 }
00514 
00515 /*UNUSED
00516 float MiniExplorerCoimbra::get_converted_robot_X_into_world(){
00517     //x world coordinate
00518     return this->map.nbCellWidth*this->map.sizeCellWidth-Y;
00519 }
00520 
00521 float MiniExplorerCoimbra::get_converted_robot_Y_into_world(){
00522     //y worldcoordinate
00523     return X;
00524 }
00525 
00526 
00527 //x and y passed are TargetNotMap
00528 float get_error_angles(float x, float y,float theta){
00529     float angleBeetweenRobotAndTarget=atan2(y,x);
00530     if(y > 0){
00531         if(angleBeetweenRobotAndTarget < PI/2)//up right
00532             angleBeetweenRobotAndTarget=-PI/2+angleBeetweenRobotAndTarget;
00533         else//up right
00534             angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-PI/2;
00535     }else{
00536         if(angleBeetweenRobotAndTarget > -PI/2)//lower right
00537             angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-PI/2;
00538         else//lower left
00539             angleBeetweenRobotAndTarget=2*PI+angleBeetweenRobotAndTarget-PI/2;
00540     }
00541     return angleBeetweenRobotAndTarget-theta;
00542 }
00543 */
00544 
00545    
00546 /*
00547 Lab4
00548 
00549 make the robot do the same square over and over, see that at one point the errors have accumulated and it is not where the odometria think it is
00550 solution:
00551 (here our sensors are bad but our odometrie s okay
00552 before each new movement we compute an estimation of where we are, compare it to what the robot think (maybe choose our estimate?)
00553 */
00554 
00555 void MiniExplorerCoimbra::test_procedure_lab_4(){
00556     this->map.fill_map_with_initial();
00557     float xEstimatedK=this->xWorld;
00558     float yEstimatedK=this->yWorld;
00559     float thetaWorldEstimatedK=this->thetaWorld;
00560     float distanceMoved;
00561     float angleMoved;
00562     float PreviousCovarianceOdometricPositionEstimate[3][3];
00563     PreviousCovarianceOdometricPositionEstimate[0][0]=0;
00564     PreviousCovarianceOdometricPositionEstimate[0][1]=0;
00565     PreviousCovarianceOdometricPositionEstimate[0][2]=0;
00566     PreviousCovarianceOdometricPositionEstimate[1][0]=0;
00567     PreviousCovarianceOdometricPositionEstimate[1][1]=0;
00568     PreviousCovarianceOdometricPositionEstimate[1][2]=0;
00569     PreviousCovarianceOdometricPositionEstimate[2][0]=0;
00570     PreviousCovarianceOdometricPositionEstimate[2][1]=0;
00571     PreviousCovarianceOdometricPositionEstimate[2][2]=0;
00572     while(1){
00573         distanceMoved=50;
00574         angleMoved=0;
00575         this->go_straight_line(50);
00576         wait(1);
00577         procedure_lab_4(xEstimatedK,yEstimatedK,thetaEstimatedK,distanceMoved,angleMoved,PreviousCovarianceOdometricPositionEstimate);
00578         pc.printf("\n\r x:%f,y:%f,theta:%f",this->xWorld,this->yWorld,this->thetaWorld);
00579         pc.printf("\n\r xEstim:%f,yEstim:%f,thetaEstim:%f",xEstimatedK,yEstimatedK,thetaEstimatedK);
00580         this->turn_to_target(PI/2);
00581         distanceMoved=0;
00582         angleMoved=PI/2;
00583         procedure_lab_4(xEstimatedK,yEstimatedK,thetaEstimatedK,distanceMoved,angleMoved,PreviousCovarianceOdometricPositionEstimate);
00584         pc.printf("\n\r x:%f,y:%f,theta:%f",this->xWorld,this->yWorld,this->thetaWorld);
00585         pc.printf("\n\r xEstim:%f,yEstim:%f,thetaEstim:%f",xEstimatedK,yEstimatedK,thetaEstimatedK);
00586     }
00587 }
00588 
00589 
00590 //compute predicted localisation (assume movements are either straight lines or pure rotation), update PreviousCovarianceOdometricPositionEstimate
00591 //TODO tweak constant rewritte it good
00592 void MiniExplorerCoimbra::procedure_lab_4(float xEstimatedK,float yEstimatedK, float thetaWorldEstimatedK, float distanceMoved, float angleMoved, float PreviousCovarianceOdometricPositionEstimate[3][3]){
00593 
00594     float distanceMovedByLeftWheel=distanceMoved/2;
00595     float distanceMovedByRightWheel=distanceMoved/2;
00596     if(angleMoved !=0){
00597         //TODO check that not sure
00598         distanceMovedByLeftWheel=-angleMoved/(2*this->distanceWheels);
00599         distanceMovedByRightWheel=angleMoved/(2*this->distanceWheels);
00600     }else{
00601         distanceMovedByLeftWheel=distanceMoved/2;
00602         distanceMovedByRightWheel=distanceMoved/2;
00603     }
00604         
00605     float xEstimatedKNext=xEstimatedK+distanceMoved*cos(angleMoved);
00606     float yEstimatedKNext=yEstimatedK+distanceMoved*sin(angleMoved);
00607     float thetaEstimatedKNext=thetaWorldEstimatedK+angleMoved;
00608     
00609     float KRight=0.1;//error constant 
00610     float KLEft=0.1;//error constant 
00611     float motionIncrementCovarianceMatrix[2][2];
00612     motionIncrementCovarianceMatrix[0][0]=kRight*abs(distanceMovedRight);
00613     motionIncrementCovarianceMatrix[0][1]=0;
00614     motionIncrementCovarianceMatrix[1][0]=0;
00615     motionIncrementCovarianceMatrix[1][1]=kLeft*abs(distanceMovedLeft);
00616     
00617     float jacobianFp[3][3];
00618     jacobianFp[0][0]=1;
00619     jacobianFp[0][1]=0;
00620     jacobianFp[0][2]=-distanceMoved*sin(thetaWorldEstimatedK+angleMoved/2);
00621     jacobianFp[1][0]=0;
00622     jacobianFp[1][1]=1;
00623     jacobianFp[1][2]=distanceMoved*cos(thetaWorldEstimatedK+angleMoved/2);;
00624     jacobianFp[2][0]=0;
00625     jacobianFp[2][1]=0;
00626     jacobianFp[2][2]=1;
00627     
00628     float jacobianFpTranspose[3][3];
00629     jacobianFpTranspose[0][0]=1;
00630     jacobianFpTranspose[0][1]=0;
00631     jacobianFpTranspose[0][2]=0;
00632     jacobianFpTranspose[1][0]=0;
00633     jacobianFpTranspose[1][1]=1;
00634     jacobianFpTranspose[1][2]=0;
00635     jacobianFpTranspose[2][0]=-distanceMoved*sin(thetaWorldEstimatedK+angleMoved/2);
00636     jacobianFpTranspose[2][1]=distanceMoved*cos(thetaWorldEstimatedK+angleMoved/2);
00637     jacobianFpTranspose[2][2]=1;
00638     
00639     float jacobianFDeltarl[3][2];
00640     jacobianFDeltarl[0][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)-(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
00641     jacobianFDeltarl[0][1]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
00642     jacobianFDeltarl[1][0]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
00643     jacobianFDeltarl[1][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)-(distancedMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
00644     jacobianFDeltarl[2][0]=1/this->distanceWheels;
00645     jacobianFDeltarl[2][1]=-1/this->distanceWheels;
00646     
00647     float jacobianFDeltarlTranspose[2][3];//1,3,5;2,4,6
00648     jacobianFDeltarlTranspose[0][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)-(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
00649     jacobianFDeltarlTranspose[0][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
00650     jacobianFDeltarlTranspose[0][2]=1/this->distanceWheels;
00651     jacobianFDeltarlTranspose[1][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
00652     jacobianFDeltarlTranspose[1][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)-(distancedMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
00653     jacobianFDeltarlTranspose[1][2]=-1/this->distanceWheels;
00654     
00655     float tempMatrix3_3[3][3];
00656     for(i = 0; i < 3; ++i){//mult 3*3, 3*3
00657         for(j = 0; j < 3; ++j){
00658             for(k = 0; k < 3; ++k){
00659                 tempMatrix3_3[i][j] += jacobianFp[i][k] * PreviousCovarianceOdometricPositionEstimate[k][j];
00660             }
00661         }
00662     }
00663     float sndTempMatrix3_3[3][3];
00664     for(i = 0; i < 3; ++i){//mult 3*3, 3*3
00665         for(j = 0; j < 3; ++j){
00666             for(k = 0; k < 3; ++k){
00667                 sndTempMatrix3_3[i][j] += tempMatrix3_3[i][k] * jacobianFpTranspose[k][j];
00668             }
00669         }
00670     }
00671     float tempMatrix3_2[3][2];
00672     for(i = 0; i < 3; ++i){//mult 3*2 , 2,2
00673         for(j = 0; j < 2; ++j){
00674             for(k = 0; k < 2; ++k){
00675                 tempMatrix3_2[i][j] += jacobianFDeltarl[i][k] * motionIncrementCovarianceMatrix[k][j];
00676             }
00677         }
00678     }
00679     float thrdTempMatrix3_3[3][3];
00680     for(i = 0; i < 3; ++i){//mult 3*2 , 2,3
00681         for(j = 0; j < 3; ++j){
00682             for(k = 0; k < 2; ++k){
00683                 thrdTempMatrix3_3[i][j] += tempMatrix3_2[i][k] * jacobianFDeltarlTranspose[k][j];
00684             }
00685         }
00686     }
00687     float newCovarianceOdometricPositionEstimate[3][3];
00688     for(i = 0; i < 3; ++i){//add 3*3 , 3*3
00689         for(j = 0; j < 3; ++j){
00690             newCovarianceOdometricPositionEstimate[i][j]=sndTempMatrix3_3[i][j]+thrdTempMatrix3_3[i][j];
00691         }
00692     }
00693     
00694     //check measurements from sonars, see if they passed the validation gate
00695     float leftCm = get_distance_left_sensor()/10;
00696     float frontCm = get_distance_front_sensor()/10;
00697     float rightCm = get_distance_right_sensor()/10;
00698     //if superior to sonar range, put the value to sonar max range + 1
00699     if(leftCm > this->sonarLeft.maxRange)
00700         leftCm=this->sonarLeft.maxRange;
00701     if(frontCm > this->sonarFront.maxRange)
00702         frontCm=this->sonarFront.maxRange;
00703     if(rightCm > this->sonarRight.maxRange)
00704         rightCm=this->sonarRight.maxRange;
00705     //get the estimated measure we should have according to our knowledge of the map and the previously computed localisation
00706     float leftCmEstimated=this->sonarLeft.maxRange;
00707     float frontCmEstimated=this->sonarFront.maxRange;
00708     float rightCmEstimated=this->sonarRight.maxRange;
00709     float xWorldCell;
00710     float yWorldCell;
00711     float currDistance;
00712     float xClosetCellLeft;
00713     float yClosetCellLeft;
00714     float xClosetCellFront;
00715     float yClosetCellFront;
00716     float xClosetCellRight;
00717     float yClosetCellRight;
00718     for(int i=0;i<this->map.nbCellWidth;i++){
00719         for(int j=0;j<this->map.nbCellHeight;j++){
00720             //check if occupied, if not discard
00721             if(this->map.get_proba_cell(i,j)==1){
00722                 //check if in sonar range
00723                 xWorldCell=this->map.cell_width_coordinate_to_world(i);
00724                 yWorldCell=this->map.cell_height_coordinate_to_world(j);
00725                 //check left
00726                 currDistance=this->sonarLeft.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaEstimatedKNext);           
00727                 if((currDistance < this->sonarLeft.maxRange) && currDistance!=-1)
00728                     //check if distance is lower than previous, update lowest if so
00729                     if(currDistance < leftCmEstimated){
00730                         leftCmEstimated= currDistance;
00731                         xClosetCellLeft=xWorldCell;
00732                         yClosetCellLeft=yWorldCell;
00733                     }
00734                 }
00735                 //check front
00736                 currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaEstimatedKNext);           
00737                 if((currDistance < this->sonarFront.maxRange) && currDistance!=-1)
00738                     //check if distance is lower than previous, update lowest if so
00739                     if(currDistance < frontCmEstimated){
00740                         frontCmEstimated= currDistance;
00741                         xClosetCellFront=xWorldCell;
00742                         yClosetCellFront=yWorldCell;
00743                     }
00744                 }
00745                 //check right
00746                 currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaEstimatedKNext);              
00747                 if((currDistance < this->sonarRight.maxRange) && currDistance!=-1)
00748                     //check if distance is lower than previous, update lowest if so
00749                     if(currDistance < rightCmEstimated){
00750                         rightCmEstimated= currDistance;
00751                         xClosetCellRight=xWorldCell;
00752                         yClosetCellRight=yWorldCell;
00753                     }
00754                 }
00755             }
00756         }
00757     }
00758     //get the innoncation: the value of the difference between the actual measure and what we anticipated
00759     float innovationLeft=leftCm-leftCmEstimated;
00760     float innovationFront=frontCm-frontCmEstimated;
00761     float innovationRight=-rightCm-rightCmEstimated;
00762     //compute jacobian of observation
00763     float jacobianOfObservationLeft[3];
00764     float jacobianOfObservationRight[3];
00765     float jacobianOfObservationFront[3];
00766     float xSonarLeft=xEstimatedKNext+this->sonarLeft.distanceX;
00767     float ySonarLeft=yEstimatedKNext+this->sonarLeft.distanceY;
00768     //left
00769     jacobianOfObservationLeft[0]=(xSonarLeft-xClosestCellLeft)/leftCmEstimated;
00770     jacobianOfObservationLeft[1]=(ySonarLeft-yClosestCellLeft)/leftCmEstimated;
00771     jacobianOfObservationLeft[2]=(xClosestCellLeft-xSonarLeft)*(xSonarLeft*sin(thetaEstimatedKNext)+ySonarLeft*cos(thetaEstimatedKNext)+(yClosestCellLeft-ySonarLeft)*(-xSonarLeft*cos(thetaEstimatedKNext)+ySonarLeft*sin(thetaEstimatedKNext));
00772     //front
00773     float xSonarFront=xEstimatedKNext+this->sonarFront.distanceX;
00774     float ySonarFront=yEstimatedKNext+this->sonarFront.distanceY;
00775     jacobianOfObservationFront[0]=(xSonarFront-xClosestCellFront)/FrontCmEstimated;
00776     jacobianOfObservationFront[1]=(ySonarFront-yClosestCellFront)/FrontCmEstimated;
00777     jacobianOfObservationFront[2]=(xClosestCellFront-xSonarFront)*(xSonarFront*sin(thetaEstimatedKNext)+ySonarFront*cos(thetaEstimatedKNext)+(yClosestCellFront-ySonarFront)*(-xSonarFront*cos(thetaEstimatedKNext)+ySonarFront*sin(thetaEstimatedKNext));
00778     //right
00779     float xSonarRight=xEstimatedKNext+this->sonarRight.distanceX;
00780     float ySonarRight=yEstimatedKNext+this->sonarRight.distanceY;
00781     jacobianOfObservationRight[0]=(xSonarRight-xClosestCellRight)/RightCmEstimated;
00782     jacobianOfObservationRight[1]=(ySonarRight-yClosestCellRight)/RightCmEstimated;
00783     jacobianOfObservationRight[2]=(xClosestCellRight-xSonarRight)*(xSonarRight*sin(thetaEstimatedKNext)+ySonarRight*cos(thetaEstimatedKNext)+(yClosestCellRight-ySonarRight)*(-xSonarRight*cos(thetaEstimatedKNext)+ySonarRight*sin(thetaEstimatedKNext));
00784     
00785     float innovationCovarianceLeft[3][3];
00786     float innovationCovarianceFront[3][3];
00787     float innovationCovarianceRight[3][3];
00788     //left
00789     float TempMatrix3_3InnovationLeft[3];
00790     TempMatrix3_3InnovationLeft[0]=jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][0];
00791     TempMatrix3_3InnovationLeft[1]=jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][1];
00792     TempMatrix3_3InnovationLeft[2]jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][2];
00793     float innovationConvarianceLeft=TempMatrix3_3InnovationLeft[0]*jacobianOfObservationLeft[0]+TempMatrix3_3InnovationLeft[1]*jacobianOfObservationLeft[1]+TempMatrix3_3InnovationLeft[2]*jacobianOfObservationLeft[2];
00794     //front
00795     float TempMatrix3_3InnovationFront[3];
00796     TempMatrix3_3InnovationFront[0]=jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][0];
00797     TempMatrix3_3InnovationFront[1]=jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][1];
00798     TempMatrix3_3InnovationFront[2]jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][2];
00799     float innovationConvarianceFront=TempMatrix3_3InnovationFront[0]*jacobianOfObservationFront[0]+TempMatrix3_3InnovationFront[1]*jacobianOfObservationFront[1]+TempMatrix3_3InnovationFront[2]*jacobianOfObservationFront[2];
00800     //right
00801     float TempMatrix3_3InnovationRight[3];
00802     TempMatrix3_3InnovationRight[0]=jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][0];
00803     TempMatrix3_3InnovationRight[1]=jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][1];
00804     TempMatrix3_3InnovationRight[2]jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][2];
00805     float innovationConvarianceRight=TempMatrix3_3InnovationRight[0]*jacobianOfObservationRight[0]+TempMatrix3_3InnovationRight[1]*jacobianOfObservationRight[1]+TempMatrix3_3InnovationRight[2]*jacobianOfObservationRight[2];
00806     
00807     //check if it pass the validation gate 
00808     float gateScoreLeft=innovationLeft*innovationLeft/innovationConvarianceLeft;
00809     float gateScoreFront=innovationFront*innovationFront/innovationConvarianceFront;
00810     float gateScoreRight=innovationRight*innovationRight/innovationConvarianceRight;
00811     int leftPassed=0;
00812     int frontPassed=0;
00813     int rightPassed=0;
00814     int e=10;//constant 
00815     if(gateScoreLeft < e)
00816         leftPassed=1;
00817     if(gateScoreFront < e)
00818         frontPassed=10;
00819     if(gateScoreRight < e)
00820         rightPassed=100;
00821     //for those who passed
00822     //compute composite innovation
00823     float compositeInnovation[3];
00824     int nbPassed=leftPassed+frontPassed+rightPassed;
00825     switch(nbPassed){
00826         case 0://none
00827             compositeInnovation[0]=0;
00828             compositeInnovation[1]=0;
00829             compositeInnovation[2]=0;
00830             break;
00831         case 1://left
00832             compositeInnovation[0]=innovationLeft;
00833             compositeInnovation[1]=0;
00834             compositeInnovation[2]=0;
00835             break;
00836         case 10://front
00837             compositeInnovation[0]=0;
00838             compositeInnovation[1]=innovationFront;
00839             compositeInnovation[2]=0;
00840             break;
00841         case 11://left and front
00842             compositeInnovation[0]=innovationLeft;
00843             compositeInnovation[1]=innovationFront;
00844             compositeInnovation[2]=0;
00845             break;
00846         case 100://right
00847             compositeInnovation[0]=0;
00848             compositeInnovation[1]=0;
00849             compositeInnovation[2]=innovationRight;
00850             break;
00851         case 101://right and left
00852             compositeInnovation[0]=innovationLeft;
00853             compositeInnovation[1]=0;
00854             compositeInnovation[2]=innovationRight;
00855             break;
00856         case 110://right and front
00857             compositeInnovation[0]=0;
00858             compositeInnovation[1]=innovationFront;
00859             compositeInnovation[2]=innovationRight;
00860             break
00861         case 111://right front and left
00862             compositeInnovation[0]=innovationLeft;
00863             compositeInnovation[1]=innovationFront;
00864             compositeInnovation[2]=innovationRight;
00865             break;
00866     }
00867     //compute composite measurement Jacobian
00868     switch(nbPassed){
00869         case 0://none
00870             break;
00871         case 1://left
00872             //compositeMeasurementJacobian = jacobianOfObservationLeft
00873             break;
00874         case 10://front
00875             //compositeMeasurementJacobian = jacobianOfObservationFront
00876             break;
00877         case 11://left and front
00878             float compositeMeasurementJacobian[6]
00879             compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
00880             compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
00881             compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
00882             compositeMeasurementJacobian[3]= jacobianOfObservationFront[0];
00883             compositeMeasurementJacobian[4]= jacobianOfObservationFront[1];
00884             compositeMeasurementJacobian[5]= jacobianOfObservationFront[2];
00885             break;
00886         case 100://right
00887             //compositeMeasurementJacobian = jacobianOfObservationRight
00888             break;
00889         case 101://right and left
00890             float compositeMeasurementJacobian[6]
00891             compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
00892             compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
00893             compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
00894             compositeMeasurementJacobian[3]= jacobianOfObservationRight[0];
00895             compositeMeasurementJacobian[4]= jacobianOfObservationRight[1];
00896             compositeMeasurementJacobian[5]= jacobianOfObservationRight[2];
00897     
00898             break;
00899         case 110://right and front
00900             float compositeMeasurementJacobian[6]
00901             compositeMeasurementJacobian[0]= jacobianOfObservationFront[0];
00902             compositeMeasurementJacobian[1]= jacobianOfObservationFront[1];
00903             compositeMeasurementJacobian[2]= jacobianOfObservationFront[2];
00904             compositeMeasurementJacobian[3]= jacobianOfObservationRight[0];
00905             compositeMeasurementJacobian[4]= jacobianOfObservationRight[1];
00906             compositeMeasurementJacobian[5]= jacobianOfObservationRight[2];
00907     
00908             break
00909         case 111://right front and left
00910             float compositeMeasurementJacobian[9]
00911             compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
00912             compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
00913             compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
00914             compositeMeasurementJacobian[3]= jacobianOfObservationFront[0];
00915             compositeMeasurementJacobian[4]= jacobianOfObservationFront[1];
00916             compositeMeasurementJacobian[5]= jacobianOfObservationFront[2];
00917             compositeMeasurementJacobian[6]= jacobianOfObservationRight[0];
00918             compositeMeasurementJacobian[7]= jacobianOfObservationRight[1];
00919             compositeMeasurementJacobian[8]= jacobianOfObservationRight[2];
00920             break;
00921     }
00922     //compute composite innovation covariance TODO dont have time, I assume you can just multiply them, can also simply all this easily
00923     float compositeInnovationCovariance
00924     switch(nbPassed){
00925         case 0://none
00926             compositeInnovationCovariance=1;
00927             break;
00928         case 1://left
00929             compositeInnovationCovariance = innovationConvarianceLeft;
00930             
00931             break;
00932         case 10://front
00933             compositeInnovationCovariance = innovationConvarianceFront;
00934             break;
00935         case 11://left and front
00936             compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceFront;
00937     
00938             break;
00939         case 100://right
00940             compositeInnovationCovariance = innovationConvarianceRight;
00941             break;
00942         case 101://right and left
00943             compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceRight;
00944     
00945             break;
00946         case 110://right and front
00947             compositeInnovationCovariance=innovationConvarianceFront*innovationConvarianceRight;
00948     
00949     
00950             break
00951         case 111://right front and left
00952             compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceFront*innovationConvarianceRight;
00953       
00954             break;
00955     }
00956     
00957     //compute kalman gain
00958     switch(nbPassed){
00959         //mult nbSonarPassed*3 , 3*3
00960         case 0://none
00961             break;
00962         case 1://left
00963             float kalmanGain[3][3];
00964             for(i = 0; i < 3; ++i){//mult 3*3, 3*1
00965                 for(j = 0; j < 1; ++j){
00966                     for(k = 0; k < 3; ++k){
00967                         kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
00968                     }
00969                 }
00970             }
00971             
00972             break;
00973         case 10://front
00974             float kalmanGain[3][3];
00975             for(i = 0; i < 3; ++i){//mult 3*3, 3*1
00976                 for(j = 0; j < 1; ++j){
00977                     for(k = 0; k < 3; ++k){
00978                         kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
00979                     }
00980                 }
00981             }
00982             break;
00983         case 11://left and front
00984             float kalmanGain[3][3];
00985             for(i = 0; i < 3; ++i){//mult 3*3, 3*2
00986                 for(j = 0; j < 2; ++j){
00987                     for(k = 0; k < 3; ++k){
00988                         kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
00989                     }
00990                 }
00991             }
00992     
00993             break;
00994         case 100://right
00995             float kalmanGain[3][3];
00996             for(i = 0; i < 3; ++i){//mult 3*3, 3*1
00997                 for(j = 0; j < 1; ++j){
00998                     for(k = 0; k < 3; ++k){
00999                         kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
01000                     }
01001                 }
01002             }
01003             break;
01004         case 101://right and left
01005             float kalmanGain[3][3];
01006             for(i = 0; i < 3; ++i){//mult 3*3, 3*2
01007                 for(j = 0; j < 2; ++j){
01008                     for(k = 0; k < 3; ++k){
01009                         kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
01010                     }
01011                 }
01012             }
01013     
01014             break;
01015         case 110://right and front
01016             float kalmanGain[3][3];
01017             for(i = 0; i < 3; ++i){//mult 3*3, 3*2
01018                 for(j = 0; j < 2; ++j){
01019                     for(k = 0; k < 3; ++k){
01020                         kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
01021                     }
01022                 }
01023             }
01024     
01025     
01026             break
01027         case 111://right front and left
01028             float kalmanGain[3][3];
01029             for(i = 0; i < 3; ++i){//mult 3*3, 3*3
01030                 for(j = 0; j < 3; ++j){
01031                     for(k = 0; k < 3; ++k){
01032                         kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
01033                     }
01034                 }
01035             }
01036       
01037             break;
01038     }
01039     for(i = 0; i < 3; ++i){//mult 3*3, 3*3
01040         for(j = 0; j < 3; ++j){
01041             kalmanGain[i][j] = kalmanGain[i][j][i][k]/compositeInnovationCovariance;
01042         }
01043     }
01044     //compute kalman gain * composite innovation
01045     //mult 3*3 , 3*1
01046     float result3_1[3][1];
01047     for(i = 0; i < 3; ++i){//mult 3*3, 3*1
01048         for(j = 0; j < 1; ++j){
01049             for(k = 0; k < 3; ++k){
01050                 result3_1[i][j] += kalmanGain[i][k] * compositeInnovation[k];
01051             }
01052         }
01053     }
01054     //compute updated robot position estimate
01055     float xEstimatedKLast=xEstimatedKNext+result3_1[0];
01056     float yEstimatedKLast=yEstimatedKNext+result3_1[1];
01057     float thetaEstimatedKLast=thetaEstimatedKNext+result3_1[2];
01058     //store the resultant covariance for next estimation
01059     /*
01060     a b c
01061     d e f
01062     g h i
01063     
01064     a d g
01065     b e h
01066     c f i
01067     */
01068     float kalmanGainTranspose[3][3];
01069     kalmanGainTranspose[0][0]=kalmanGain[0][0];
01070     kalmanGainTranspose[0][1]=kalmanGain[1][0];
01071     kalmanGainTranspose[0][2]=kalmanGain[2][0];
01072     kalmanGainTranspose[1][0]=kalmanGain[0][1];
01073     kalmanGainTranspose[1][1]=kalmanGain[1][1];
01074     kalmanGainTranspose[1][2]=kalmanGain[2][1];
01075     kalmanGainTranspose[2][0]=kalmanGain[0][2];
01076     kalmanGainTranspose[2][1]=kalmanGain[1][2];
01077     kalmanGainTranspose[2][2]=kalmanGain[2][2];
01078     
01079     //mult 3*3 , 3*3
01080     float fithTempMatrix3_3[3][3];
01081     for(i = 0; i < 3; ++i){//mult 3*3 , 3*3
01082         for(j = 0; j < 3; ++j){
01083             for(k = 0; k < 3; ++k){
01084                 fithTempMatrix3_3[i][j] += kalmanGain[i][k] * kalmanGainTranspose[k][j];
01085             }
01086         }
01087     }
01088     for(i = 0; i < 3; ++i){//add 3*3 , 3*3
01089         for(j = 0; j < 3; ++j){
01090             fithTempMatrix3_3[i][j]=fithTempMatrix3_3[i][j]*compositeInnovationCovariance;
01091         }
01092     }
01093     float covariancePositionEsimatedLast[3][3];
01094     for(i = 0; i < 3; ++i){//add 3*3 , 3*3
01095         for(j = 0; j < 3; ++j){
01096             covariancePositionEsimatedLast[i][j]=fithTempMatrix3_3[i][j]+newCovarianceOdometricPositionEstimate[i][j];
01097         }
01098     }
01099     //update PreviousCovarianceOdometricPositionEstimate
01100     for(i = 0; i < 3; ++i){
01101         for(j = 0; j < 3; ++j){
01102             PreviousCovarianceOdometricPositionEstimate[i][j]=covariancePositionEsimatedLast[i][j];
01103         }
01104     }
01105     
01106     xEstimatedK=xEstimatedKLast;
01107     yEstimatedK=yEstimatedKLast;
01108     thetaEstimatedK=thetaEstimatedKLast;
01109 }