Yo Fleyt
test morning
Dependencies: ISR_Mini-explorer mbed
Fork of
roboticLab_withclass_3_July
by 
Georgios Tsamis
Diff: MiniExplorerCoimbra.cpp
- Revision:
- 2:11cd5173aa36
- Parent:
- 1:20f48907c726
- Child:
- 3:37345c109dfc
--- a/MiniExplorerCoimbra.cpp Mon Jul 03 17:06:16 2017 +0000
+++ b/MiniExplorerCoimbra.cpp Wed Jul 05 08:56:12 2017 +0000
@@ -3,7 +3,7 @@
#define PI 3.14159
-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){
+MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld, float widthRealMap, float heightRealMap):map(widthRealMap,heightRealMap,20,20),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){
i2c1.frequency(100000);
initRobot(); //Initializing the robot
pc.baud(9600); // baud for the pc communication
@@ -115,11 +115,18 @@
rightMotor(1,0);
}
+void MiniExplorerCoimbra::test_procedure_lab2(int nbIteration){
+ for(int i=0;i<nbIteration;i++){
+ this->randomize_and_map();
+ this->print_map_with_robot_position();
+ }
+}
+
//generate a position randomly and makes the robot go there while updating the map
void MiniExplorerCoimbra::randomize_and_map() {
//TODO check that it's aurelien's work
- float movementOnX=rand()%(int)(this->map.widthRealMap/2);
- float movementOnY=rand()%(int)(this->map.heightRealMap/2);
+ float movementOnX=rand()%(int)(this->map.widthRealMap);
+ float movementOnY=rand()%(int)(this->map.heightRealMap);
float signOfX=rand()%2;
if(signOfX < 1)
@@ -127,13 +134,30 @@
float signOfY=rand()%2;
if(signOfY < 1)
signOfY=-1;
-
+
float targetXWorld = movementOnX*signOfX;
float targetYWorld = movementOnY*signOfY;
float targetAngleWorld = 2*((float)(rand()%31416)-15708)/10000.0;
this->go_to_point_with_angle(targetXWorld, targetYWorld, targetAngleWorld);
}
+void MiniExplorerCoimbra::test_sonars_and_map(int nbIteration){
+ float leftMm;
+ float frontMm;
+ float rightMm;
+ this->myOdometria();
+ this->print_map_with_robot_position();
+ for(int i=0;i<nbIteration;i++){
+ leftMm = get_distance_left_sensor();
+ frontMm = get_distance_front_sensor();
+ rightMm = get_distance_right_sensor();
+ this->update_sonar_values(leftMm, frontMm, rightMm);
+ this->print_map_with_robot_position();
+ }
+}
+
+
+//generate a position randomly and makes the robot go there while updating the map
//move of targetXWorld and targetYWorld ending in a targetAngleWorld
void MiniExplorerCoimbra::go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld) {
bool keepGoing=true;
@@ -151,40 +175,37 @@
//Updating X,Y and theta with the odometry values
this->myOdometria();
- leftMm = get_distance_left_sensor();
- frontMm = get_distance_front_sensor();
- rightMm = get_distance_right_sensor();
-
- //if in dangerzone
+ leftMm = get_distance_left_sensor();
+ frontMm = get_distance_front_sensor();
+ rightMm = get_distance_right_sensor();
+ //if in dangerzone 150 mm
if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){
+ //stop motors
leftMotor(1,0);
rightMotor(1,0);
+ //update the map
this->update_sonar_values(leftMm, frontMm, rightMm);
- //TODO Giorgos maybe you can also test the do_half_flip() function
this->myOdometria();
- //do a flip TODO
keepGoing=false;
- this->do_half_flip();
+ this->do_half_flip();
}else{
//if not in danger zone continue as usual
this->update_sonar_values(leftMm, frontMm, rightMm);
-
- //Updating motor velocities
+ //Updating motor velocities
distanceToTarget=this->update_angular_speed_wheels_go_to_point_with_angle(targetXWorld,targetYWorld,targetAngleWorld,dt);
-
wait(0.2);
//Timer stuff
t.stop();
- pc.printf("\n\r dist to target= %f",distanceToTarget);
+ pc.printf("\n\rdist to target= %f",distanceToTarget);
}
- } while(distanceToTarget>1 && (abs(targetAngleWorld-this->thetaWorld)>0.01) && keepGoing);
+ } while((distanceToTarget>1 || (abs(targetAngleWorld-this->thetaWorld)>0.1)) && keepGoing);
//Stop at the end
leftMotor(1,0);
rightMotor(1,0);
+ pc.printf("\r\nReached Target!");
}
-
//move of targetXWorld and targetYWorld ending in a targetAngleWorld
void MiniExplorerCoimbra::go_to_point_with_angle_first_lab(float targetXWorld, float targetYWorld, float targetAngleWorld) {
float dt;
@@ -198,7 +219,6 @@
//Updating X,Y and theta with the odometry values
this->myOdometria();
-
//Updating motor velocities
distanceToTarget=this->update_angular_speed_wheels_go_to_point_with_angle(targetXWorld,targetYWorld,targetAngleWorld,dt);
@@ -431,6 +451,40 @@
}
}
+void MiniExplorerCoimbra::print_map_with_robot_position(){
+ float currProba;
+
+ float heightIndiceInOrthonormal;
+ float widthIndiceInOrthonormal;
+
+ float widthMalus=-(3*this->map.sizeCellWidth/2);
+ float widthBonus=this->map.sizeCellWidth/2;
+
+ float heightMalus=-(3*this->map.sizeCellHeight/2);
+ float heightBonus=this->map.sizeCellHeight/2;
+
+ pc.printf("\n\r");
+ for (int y = this->map.nbCellHeight -1; y>-1; y--) {
+ for (int x= 0; x<this->map.nbCellWidth; x++) {
+ heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
+ widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
+ if(this->yWorld >= (heightIndiceInOrthonormal+heightMalus) && this->yWorld <= (heightIndiceInOrthonormal+heightBonus) && this->xWorld >= (widthIndiceInOrthonormal+widthMalus) && this->xWorld <= (widthIndiceInOrthonormal+widthBonus))
+ pc.printf(" R ");
+ else{
+ currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
+ if ( currProba < 0.5)
+ pc.printf(" ");
+ else{
+ if(currProba==0.5)
+ pc.printf(" . ");
+ else
+ pc.printf(" X ");
+ }
+ }
+ }
+ pc.printf("\n\r");
+ }
+}
//robotX and robotY are from this->myOdometria(), calculate line_a, line_b and line_c
void MiniExplorerCoimbra::calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c){
@@ -480,50 +534,42 @@
}
}
-//currently line_c is not used
+//for vff
void MiniExplorerCoimbra::go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld){
float lineAngle;
float angleError;
float linear;
float angular;
-
- //atan2 use the deplacement on X and the deplacement on Y
- float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
- bool aligned=false;
-
- //this condition is passed if the target is in the same direction as the robot orientation
- if((angleToTarget>0 && this->thetaWorld > 0) || (angleToTarget<0 && this->thetaWorld <0))
- aligned=true;
+ float d;
//line angle is beetween pi/2 and -pi/2
- /*
- ax+by+c=0 (here c==0)
- y=x*-a/b
- if a*b > 0, the robot is going down
- if a*b <0, the robot is going up
- */
- if(line_b!=0){
- if(!aligned)
- lineAngle=atan(-line_a/line_b);
- else
- lineAngle=atan(line_a/-line_b);
+
+ if(line_b!=0){
+ lineAngle=atan(line_a/-line_b);
}
else{
lineAngle=1.5708;
}
- //Computing angle error
- angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
- angleError = atan(sin(angleError)/cos(angleError));
+ this->myOdometria();
+ //Computing angle error
+ angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
+ if(angleError>PI)
+ angleError=-(angleError-PI);
+ else
+ if(angleError<-PI)
+ angleError=-(angleError+PI);
+
+ d=this->distFromLine(this->xWorld, this->yWorld, line_a, line_b, line_c);//this could be 0
//Calculating velocities
- linear=this->kv*(3.1416);
- angular=this->kh*angleError;
+ linear= this->kv*(3.14);
+ angular=-this->kd*d + this->kh*angleError;
float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
- float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
+ float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
- //Normalize speed for motors
+ //Normalize speed for motors
if(abs(angularLeft)>abs(angularRight)) {
angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
angularLeft=this->speed*this->sign1(angularLeft);
@@ -532,6 +578,10 @@
angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
angularRight=this->speed*this->sign1(angularRight);
}
+
+ pc.printf("\r\nd = %f", d);
+ pc.printf("\r\nerror = %f\n", angleError);
+
leftMotor(this->sign2(angularLeft),abs(angularLeft));
rightMotor(this->sign2(angularRight),abs(angularRight));
}
@@ -621,559 +671,3 @@
return abs((line_a*robot_x+line_b*robot_y+line_c)/sqrt(line_a*line_a+line_b*line_b));
}
-/*
-void MiniExplorerCoimbra::test_procedure_lab_4(){
- this->map.fill_map_with_initial();
- float xEstimatedK=this->xWorld;
- float yEstimatedK=this->yWorld;
- float thetaWorldEstimatedK = this->thetaWorld;
- float distanceMoved;
- float angleMoved;
- float PreviousCovarianceOdometricPositionEstimate[3][3];
- PreviousCovarianceOdometricPositionEstimate[0][0]=0;
- PreviousCovarianceOdometricPositionEstimate[0][1]=0;
- PreviousCovarianceOdometricPositionEstimate[0][2]=0;
- PreviousCovarianceOdometricPositionEstimate[1][0]=0;
- PreviousCovarianceOdometricPositionEstimate[1][1]=0;
- PreviousCovarianceOdometricPositionEstimate[1][2]=0;
- PreviousCovarianceOdometricPositionEstimate[2][0]=0;
- PreviousCovarianceOdometricPositionEstimate[2][1]=0;
- PreviousCovarianceOdometricPositionEstimate[2][2]=0;
- while(1){
- distanceMoved=50;
- angleMoved=0;
- this->go_straight_line(50);
- wait(1);
- procedure_lab_4(xEstimatedK,yEstimatedK,thetaWorldEstimatedK,distanceMoved,angleMoved,PreviousCovarianceOdometricPositionEstimate);
- pc.printf("\n\r x:%f,y:%f,theta:%f",this->xWorld,this->yWorld,this->thetaWorld);
- pc.printf("\n\r xEstim:%f,yEstim:%f,thetaEstim:%f",xEstimatedK,yEstimatedK,thetaWorldEstimatedK);
- this->turn_to_target(PI/2);
- distanceMoved=0;
- angleMoved=PI/2;
- procedure_lab_4(xEstimatedK,yEstimatedK,thetaWorldEstimatedK,distanceMoved,angleMoved,PreviousCovarianceOdometricPositionEstimate);
- pc.printf("\n\r x:%f,y:%f,theta:%f",this->xWorld,this->yWorld,this->thetaWorld);
- pc.printf("\n\r xEstim:%f,yEstim:%f,thetaEstim:%f",xEstimatedK,yEstimatedK,thetaWorldEstimatedK);
- }
-}
-
-void MiniExplorerCoimbra::go_straight_line(float distanceCm){
- leftMotor(1,1);
- rightMotor(1,1);
- float xEstimated=this->xWorld+distanceCm*cos(this->thetaWorld);
- float yEstimated=this->yWorld+distanceCm*sin(this->thetaWorld);
- while(1){
- this->myOdometria();
- if(abs(xEstimated-this->xWorld) < 0.1 && abs(yEstimated-this->yWorld) < 0.1)
- break;
- }
- leftMotor(1,0);
- rightMotor(1,0);
-}
-
-//compute predicted localisation (assume movements are either straight lines or pure rotation), update PreviousCovarianceOdometricPositionEstimate
-//TODO tweak constant rewritte it good
-void MiniExplorerCoimbra::procedure_lab_4(float xEstimatedK,float yEstimatedK, float thetaWorldEstimatedK, float distanceMoved, float angleMoved, float PreviousCovarianceOdometricPositionEstimate[3][3]){
-
- float distanceMovedByLeftWheel=distanceMoved/2;
- float distanceMovedByRightWheel=distanceMoved/2;
- if(angleMoved !=0){
- //TODO check that not sure
- distanceMovedByLeftWheel=-angleMoved/(2*this->distanceWheels);
- distanceMovedByRightWheel=angleMoved/(2*this->distanceWheels);
- }else{
- distanceMovedByLeftWheel=distanceMoved/2;
- distanceMovedByRightWheel=distanceMoved/2;
- }
-
- float xEstimatedKNext=xEstimatedK+distanceMoved*cos(angleMoved);
- float yEstimatedKNext=yEstimatedK+distanceMoved*sin(angleMoved);
- float thetaWorldEstimatedKNext=thetaWorldEstimatedK+angleMoved;
-
- float kRight=0.1;//error constant
- float kLeft=0.1;//error constant
- float motionIncrementCovarianceMatrix[2][2];
- motionIncrementCovarianceMatrix[0][0]=kRight*abs(distanceMovedByRightWheel);
- motionIncrementCovarianceMatrix[0][1]=0;
- motionIncrementCovarianceMatrix[1][0]=0;
- motionIncrementCovarianceMatrix[1][1]=kLeft*abs(distanceMovedByLeftWheel);
-
- float jacobianFp[3][3];
- jacobianFp[0][0]=1;
- jacobianFp[0][1]=0;
- jacobianFp[0][2]=-distanceMoved*sin(thetaWorldEstimatedK+angleMoved/2);
- jacobianFp[1][0]=0;
- jacobianFp[1][1]=1;
- jacobianFp[1][2]=distanceMoved*cos(thetaWorldEstimatedK+angleMoved/2);;
- jacobianFp[2][0]=0;
- jacobianFp[2][1]=0;
- jacobianFp[2][2]=1;
-
- float jacobianFpTranspose[3][3];
- jacobianFpTranspose[0][0]=1;
- jacobianFpTranspose[0][1]=0;
- jacobianFpTranspose[0][2]=0;
- jacobianFpTranspose[1][0]=0;
- jacobianFpTranspose[1][1]=1;
- jacobianFpTranspose[1][2]=0;
- jacobianFpTranspose[2][0]=-distanceMoved*sin(thetaWorldEstimatedK+angleMoved/2);
- jacobianFpTranspose[2][1]=distanceMoved*cos(thetaWorldEstimatedK+angleMoved/2);
- jacobianFpTranspose[2][2]=1;
-
- float jacobianFDeltarl[3][2];
- jacobianFDeltarl[0][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)-(distanceMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
- jacobianFDeltarl[0][1]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)+(distanceMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
- jacobianFDeltarl[1][0]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)+(distanceMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
- jacobianFDeltarl[1][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)-(distanceMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
- jacobianFDeltarl[2][0]=1/this->distanceWheels;
- jacobianFDeltarl[2][1]=-1/this->distanceWheels;
-
- float jacobianFDeltarlTranspose[2][3];//1,3,5;2,4,6
- jacobianFDeltarlTranspose[0][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)-(distanceMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
- jacobianFDeltarlTranspose[0][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)+(distanceMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
- jacobianFDeltarlTranspose[0][2]=1/this->distanceWheels;
- jacobianFDeltarlTranspose[1][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)+(distanceMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
- jacobianFDeltarlTranspose[1][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)-(distanceMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
- jacobianFDeltarlTranspose[1][2]=-1/this->distanceWheels;
-
- float tempMatrix3_3[3][3];
- int i;
- int j;
- int k;
- for( i = 0; i < 3; ++i){//mult 3*3, 3*3
- for(j = 0; j < 3; ++j){
- for(k = 0; k < 3; ++k){
- tempMatrix3_3[i][j] += jacobianFp[i][k] * PreviousCovarianceOdometricPositionEstimate[k][j];
- }
- }
- }
- float sndTempMatrix3_3[3][3];
- for(i = 0; i < 3; ++i){//mult 3*3, 3*3
- for(j = 0; j < 3; ++j){
- for(k = 0; k < 3; ++k){
- sndTempMatrix3_3[i][j] += tempMatrix3_3[i][k] * jacobianFpTranspose[k][j];
- }
- }
- }
- float tempMatrix3_2[3][2];
- for(i = 0; i < 3; ++i){//mult 3*2 , 2,2
- for(j = 0; j < 2; ++j){
- for(k = 0; k < 2; ++k){
- tempMatrix3_2[i][j] += jacobianFDeltarl[i][k] * motionIncrementCovarianceMatrix[k][j];
- }
- }
- }
- float thrdTempMatrix3_3[3][3];
- for(i = 0; i < 3; ++i){//mult 3*2 , 2,3
- for(j = 0; j < 3; ++j){
- for(k = 0; k < 2; ++k){
- thrdTempMatrix3_3[i][j] += tempMatrix3_2[i][k] * jacobianFDeltarlTranspose[k][j];
- }
- }
- }
- float newCovarianceOdometricPositionEstimate[3][3];
- for(i = 0; i < 3; ++i){//add 3*3 , 3*3
- for(j = 0; j < 3; ++j){
- newCovarianceOdometricPositionEstimate[i][j]=sndTempMatrix3_3[i][j]+thrdTempMatrix3_3[i][j];
- }
- }
-
- //check measurements from sonars, see if they passed the validation gate
- float leftCm = get_distance_left_sensor()/10;
- float frontCm = get_distance_front_sensor()/10;
- float rightCm = get_distance_right_sensor()/10;
- //if superior to sonar range, put the value to sonar max range + 1
- if(leftCm > this->sonarLeft.maxRange)
- leftCm=this->sonarLeft.maxRange;
- if(frontCm > this->sonarFront.maxRange)
- frontCm=this->sonarFront.maxRange;
- if(rightCm > this->sonarRight.maxRange)
- rightCm=this->sonarRight.maxRange;
- //get the estimated measure we should have according to our knowledge of the map and the previously computed localisation
- float leftCmEstimated=this->sonarLeft.maxRange;
- float frontCmEstimated=this->sonarFront.maxRange;
- float rightCmEstimated=this->sonarRight.maxRange;
- float xWorldCell;
- float yWorldCell;
- float currDistance;
- float xClosestCellLeft;
- float yClosestCellLeft;
- float xClosestCellFront;
- float yClosestCellFront;
- float xClosestCellRight;
- float yClosestCellRight;
- for(int i=0;i<this->map.nbCellWidth;i++){
- for(int j=0;j<this->map.nbCellHeight;j++){
- //check if occupied, if not discard
- if(this->map.get_proba_cell(i,j)==1){
- //check if in sonar range
- xWorldCell=this->map.cell_width_coordinate_to_world(i);
- yWorldCell=this->map.cell_height_coordinate_to_world(j);
- //check left
- currDistance=this->sonarLeft.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaWorldEstimatedKNext);
- if((currDistance < this->sonarLeft.maxRange) && currDistance!=-1){
- //check if distance is lower than previous, update lowest if so
- if(currDistance < leftCmEstimated){
- leftCmEstimated= currDistance;
- xClosestCellLeft=xWorldCell;
- yClosestCellLeft=yWorldCell;
- }
- }
- //check front
- currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaWorldEstimatedKNext);
- if((currDistance < this->sonarFront.maxRange) && currDistance!=-1){
- //check if distance is lower than previous, update lowest if so
- if(currDistance < frontCmEstimated){
- frontCmEstimated= currDistance;
- xClosestCellFront=xWorldCell;
- yClosestCellFront=yWorldCell;
- }
- }
- //check right
- currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaWorldEstimatedKNext);
- if((currDistance < this->sonarRight.maxRange) && currDistance!=-1){
- //check if distance is lower than previous, update lowest if so
- if(currDistance < rightCmEstimated){
- rightCmEstimated= currDistance;
- xClosestCellRight=xWorldCell;
- yClosestCellRight=yWorldCell;
- }
- }
- }
- }
- }
- //get the innoncation: the value of the difference between the actual measure and what we anticipated
- float innovationLeft=leftCm-leftCmEstimated;
- float innovationFront=frontCm-frontCmEstimated;
- float innovationRight=-rightCm-rightCmEstimated;
- //compute jacobian of observation
- float jacobianOfObservationLeft[3];
- float jacobianOfObservationRight[3];
- float jacobianOfObservationFront[3];
- float xSonarLeft=xEstimatedKNext+this->sonarLeft.distanceX;
- float ySonarLeft=yEstimatedKNext+this->sonarLeft.distanceY;
- //left
- jacobianOfObservationLeft[0]=(xSonarLeft-xClosestCellLeft)/leftCmEstimated;
- jacobianOfObservationLeft[1]=(ySonarLeft-yClosestCellLeft)/leftCmEstimated;
- jacobianOfObservationLeft[2]=(xClosestCellLeft-xSonarLeft)*(xSonarLeft*sin(thetaWorldEstimatedKNext)+ySonarLeft*cos(thetaWorldEstimatedKNext))+(yClosestCellLeft-ySonarLeft)*(-xSonarLeft*cos(thetaWorldEstimatedKNext)+ySonarLeft*sin(thetaWorldEstimatedKNext));
- //front
- float xSonarFront=xEstimatedKNext+this->sonarFront.distanceX;
- float ySonarFront=yEstimatedKNext+this->sonarFront.distanceY;
- jacobianOfObservationFront[0]=(xSonarFront-xClosestCellFront)/frontCmEstimated;
- jacobianOfObservationFront[1]=(ySonarFront-yClosestCellFront)/frontCmEstimated;
- jacobianOfObservationFront[2]=(xClosestCellFront-xSonarFront)*(xSonarFront*sin(thetaWorldEstimatedKNext)+ySonarFront*cos(thetaWorldEstimatedKNext))+(yClosestCellFront-ySonarFront)*(-xSonarFront*cos(thetaWorldEstimatedKNext)+ySonarFront*sin(thetaWorldEstimatedKNext));
- //right
- float xSonarRight=xEstimatedKNext+this->sonarRight.distanceX;
- float ySonarRight=yEstimatedKNext+this->sonarRight.distanceY;
- jacobianOfObservationRight[0]=(xSonarRight-xClosestCellRight)/rightCmEstimated;
- jacobianOfObservationRight[1]=(ySonarRight-yClosestCellRight)/rightCmEstimated;
- jacobianOfObservationRight[2]=(xClosestCellRight-xSonarRight)*(xSonarRight*sin(thetaWorldEstimatedKNext)+ySonarRight*cos(thetaWorldEstimatedKNext))+(yClosestCellRight-ySonarRight)*(-xSonarRight*cos(thetaWorldEstimatedKNext)+ySonarRight*sin(thetaWorldEstimatedKNext));
-
- //left
- float TempMatrix3_3InnovationLeft[3];
- TempMatrix3_3InnovationLeft[0]=jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][0];
- TempMatrix3_3InnovationLeft[1]=jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][1];
- TempMatrix3_3InnovationLeft[2]=jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][2];
- float innovationConvarianceLeft=TempMatrix3_3InnovationLeft[0]*jacobianOfObservationLeft[0]+TempMatrix3_3InnovationLeft[1]*jacobianOfObservationLeft[1]+TempMatrix3_3InnovationLeft[2]*jacobianOfObservationLeft[2];
- //front
- float TempMatrix3_3InnovationFront[3];
- TempMatrix3_3InnovationFront[0]=jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][0];
- TempMatrix3_3InnovationFront[1]=jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][1];
- TempMatrix3_3InnovationFront[2]=jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][2];
- float innovationConvarianceFront=TempMatrix3_3InnovationFront[0]*jacobianOfObservationFront[0]+TempMatrix3_3InnovationFront[1]*jacobianOfObservationFront[1]+TempMatrix3_3InnovationFront[2]*jacobianOfObservationFront[2];
- //right
- float TempMatrix3_3InnovationRight[3];
- TempMatrix3_3InnovationRight[0]=jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][0];
- TempMatrix3_3InnovationRight[1]=jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][1];
- TempMatrix3_3InnovationRight[2]=jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][2];
- float innovationConvarianceRight=TempMatrix3_3InnovationRight[0]*jacobianOfObservationRight[0]+TempMatrix3_3InnovationRight[1]*jacobianOfObservationRight[1]+TempMatrix3_3InnovationRight[2]*jacobianOfObservationRight[2];
-
- //check if it pass the validation gate
- float gateScoreLeft=innovationLeft*innovationLeft/innovationConvarianceLeft;
- float gateScoreFront=innovationFront*innovationFront/innovationConvarianceFront;
- float gateScoreRight=innovationRight*innovationRight/innovationConvarianceRight;
- int leftPassed=0;
- int frontPassed=0;
- int rightPassed=0;
- int e=10;//constant
- if(gateScoreLeft < e)
- leftPassed=1;
- if(gateScoreFront < e)
- frontPassed=10;
- if(gateScoreRight < e)
- rightPassed=100;
- //for those who passed
- //compute composite innovation
- float compositeInnovation[3];
- int nbPassed=leftPassed+frontPassed+rightPassed;
- switch(nbPassed){
- case 0://none
- compositeInnovation[0]=0;
- compositeInnovation[1]=0;
- compositeInnovation[2]=0;
- break;
- case 1://left
- compositeInnovation[0]=innovationLeft;
- compositeInnovation[1]=0;
- compositeInnovation[2]=0;
- break;
- case 10://front
- compositeInnovation[0]=0;
- compositeInnovation[1]=innovationFront;
- compositeInnovation[2]=0;
- break;
- case 11://left and front
- compositeInnovation[0]=innovationLeft;
- compositeInnovation[1]=innovationFront;
- compositeInnovation[2]=0;
- break;
- case 100://right
- compositeInnovation[0]=0;
- compositeInnovation[1]=0;
- compositeInnovation[2]=innovationRight;
- break;
- case 101://right and left
- compositeInnovation[0]=innovationLeft;
- compositeInnovation[1]=0;
- compositeInnovation[2]=innovationRight;
- break;
- case 110://right and front
- compositeInnovation[0]=0;
- compositeInnovation[1]=innovationFront;
- compositeInnovation[2]=innovationRight;
- break;
- case 111://right front and left
- compositeInnovation[0]=innovationLeft;
- compositeInnovation[1]=innovationFront;
- compositeInnovation[2]=innovationRight;
- break;
- }
- //compute composite measurement Jacobian
- float *compositeMeasurementJacobian;
- switch(nbPassed){
- case 0://none
- break;
- case 1://left
- //compositeMeasurementJacobian = jacobianOfObservationLeft
- break;
- case 10://front
- //compositeMeasurementJacobian = jacobianOfObservationFront
- break;
- case 11://left and front
- compositeMeasurementJacobian=new float[6];
- compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
- compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
- compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
- compositeMeasurementJacobian[3]= jacobianOfObservationFront[0];
- compositeMeasurementJacobian[4]= jacobianOfObservationFront[1];
- compositeMeasurementJacobian[5]= jacobianOfObservationFront[2];
- break;
- case 100://right
- //compositeMeasurementJacobian = jacobianOfObservationRight
- break;
- case 101://right and left
- compositeMeasurementJacobian=new float[6];
- compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
- compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
- compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
- compositeMeasurementJacobian[3]= jacobianOfObservationRight[0];
- compositeMeasurementJacobian[4]= jacobianOfObservationRight[1];
- compositeMeasurementJacobian[5]= jacobianOfObservationRight[2];
-
- break;
- case 110://right and front
- compositeMeasurementJacobian=new float[6];
- compositeMeasurementJacobian[0]= jacobianOfObservationFront[0];
- compositeMeasurementJacobian[1]= jacobianOfObservationFront[1];
- compositeMeasurementJacobian[2]= jacobianOfObservationFront[2];
- compositeMeasurementJacobian[3]= jacobianOfObservationRight[0];
- compositeMeasurementJacobian[4]= jacobianOfObservationRight[1];
- compositeMeasurementJacobian[5]= jacobianOfObservationRight[2];
-
- break;
- case 111://right front and left
- compositeMeasurementJacobian=new float[9];
- compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
- compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
- compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
- compositeMeasurementJacobian[3]= jacobianOfObservationFront[0];
- compositeMeasurementJacobian[4]= jacobianOfObservationFront[1];
- compositeMeasurementJacobian[5]= jacobianOfObservationFront[2];
- compositeMeasurementJacobian[6]= jacobianOfObservationRight[0];
- compositeMeasurementJacobian[7]= jacobianOfObservationRight[1];
- compositeMeasurementJacobian[8]= jacobianOfObservationRight[2];
- break;
- }
- //compute composite innovation covariance TODO dont have time, I assume you can just multiply them, can also simply all this easily
- float compositeInnovationCovariance;
- switch(nbPassed){
- case 0://none
- compositeInnovationCovariance=1;
- break;
- case 1://left
- compositeInnovationCovariance = innovationConvarianceLeft;
-
- break;
- case 10://front
- compositeInnovationCovariance = innovationConvarianceFront;
- break;
- case 11://left and front
- compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceFront;
-
- break;
- case 100://right
- compositeInnovationCovariance = innovationConvarianceRight;
- break;
- case 101://right and left
- compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceRight;
- break;
- case 110://right and front
- compositeInnovationCovariance=innovationConvarianceFront*innovationConvarianceRight;
-
-
- break;
- case 111://right front and left
- compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceFront*innovationConvarianceRight;
-
- break;
- }
-
- //compute kalman gain
- float kalmanGain[3][3];
- switch(nbPassed){
- //mult nbSonarPassed*3 , 3*3
- case 0://none
- break;
- case 1://left
-
- for(i = 0; i < 3; ++i){//mult 3*3, 3*1
- for(j = 0; j < 1; ++j){
- for(k = 0; k < 3; ++k){
- kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
- }
- }
- }
-
- break;
- case 10://front
- for(i = 0; i < 3; ++i){//mult 3*3, 3*1
- for(j = 0; j < 1; ++j){
- for(k = 0; k < 3; ++k){
- kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
- }
- }
- }
- break;
- case 11://left and front
- for(i = 0; i < 3; ++i){//mult 3*3, 3*2
- for(j = 0; j < 2; ++j){
- for(k = 0; k < 3; ++k){
- kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
- }
- }
- }
-
- break;
- case 100://right
- for(i = 0; i < 3; ++i){//mult 3*3, 3*1
- for(j = 0; j < 1; ++j){
- for(k = 0; k < 3; ++k){
- kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
- }
- }
- }
- break;
- case 101://right and left
- for(i = 0; i < 3; ++i){//mult 3*3, 3*2
- for(j = 0; j < 2; ++j){
- for(k = 0; k < 3; ++k){
- kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
- }
- }
- }
-
- break;
- case 110://right and front
- for(i = 0; i < 3; ++i){//mult 3*3, 3*2
- for(j = 0; j < 2; ++j){
- for(k = 0; k < 3; ++k){
- kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
- }
- }
- }
-
-
- break;
- case 111://right front and left
- for(i = 0; i < 3; ++i){//mult 3*3, 3*3
- for(j = 0; j < 3; ++j){
- for(k = 0; k < 3; ++k){
- kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
- }
- }
- }
-
- break;
- }
-
- for(i = 0; i < 3; ++i){//mult 3*3, 3*3
- for(j = 0; j < 3; ++j){
- kalmanGain[i][j] = kalmanGain[i][j]/compositeInnovationCovariance;
- }
- }
- //compute kalman gain * composite innovation
- //mult 3*3 , 3*1
- float result3_1[3];
- for(i = 0; i < 3; ++i){//mult 3*3, 3*1
- for(k = 0; k < 3; ++k){
- result3_1[i] += kalmanGain[i][k] * compositeInnovation[k];
- }
- }
- //compute updated robot position estimate
- float xEstimatedKLast=xEstimatedKNext+result3_1[0];
- float yEstimatedKLast=yEstimatedKNext+result3_1[1];
- float thetaWorldEstimatedKLast=thetaWorldEstimatedKNext+result3_1[2];
- //store the resultant covariance for next estimation
-
- float kalmanGainTranspose[3][3];
- kalmanGainTranspose[0][0]=kalmanGain[0][0];
- kalmanGainTranspose[0][1]=kalmanGain[1][0];
- kalmanGainTranspose[0][2]=kalmanGain[2][0];
- kalmanGainTranspose[1][0]=kalmanGain[0][1];
- kalmanGainTranspose[1][1]=kalmanGain[1][1];
- kalmanGainTranspose[1][2]=kalmanGain[2][1];
- kalmanGainTranspose[2][0]=kalmanGain[0][2];
- kalmanGainTranspose[2][1]=kalmanGain[1][2];
- kalmanGainTranspose[2][2]=kalmanGain[2][2];
-
- //mult 3*3 , 3*3
- float fithTempMatrix3_3[3][3];
- for(i = 0; i < 3; ++i){//mult 3*3 , 3*3
- for(j = 0; j < 3; ++j){
- for(k = 0; k < 3; ++k){
- fithTempMatrix3_3[i][j] += kalmanGain[i][k] * kalmanGainTranspose[k][j];
- }
- }
- }
- for(i = 0; i < 3; ++i){//add 3*3 , 3*3
- for(j = 0; j < 3; ++j){
- fithTempMatrix3_3[i][j]=fithTempMatrix3_3[i][j]*compositeInnovationCovariance;
- }
- }
- float covariancePositionEsimatedLast[3][3];
- for(i = 0; i < 3; ++i){//add 3*3 , 3*3
- for(j = 0; j < 3; ++j){
- covariancePositionEsimatedLast[i][j]=fithTempMatrix3_3[i][j]+newCovarianceOdometricPositionEstimate[i][j];
- }
- }
- //update PreviousCovarianceOdometricPositionEstimate
- for(i = 0; i < 3; ++i){
- for(j = 0; j < 3; ++j){
- PreviousCovarianceOdometricPositionEstimate[i][j]=covariancePositionEsimatedLast[i][j];
- }
- }
-
- xEstimatedK=xEstimatedKLast;
- yEstimatedK=yEstimatedKLast;
- thetaWorldEstimatedK=thetaWorldEstimatedKLast;
-}
-*/
\ No newline at end of file