Yo Fleyt
with class
Dependencies: ISR_Mini-explorer mbed
Fork of
VirtualForces
by 
Georgios Tsamis
Diff: MiniExplorerCoimbra.cpp
- Revision:
- 39:890439b495e3
- Parent:
- 38:5ed7c79fb724
--- a/MiniExplorerCoimbra.cpp Thu Jun 15 23:17:55 2017 +0000
+++ b/MiniExplorerCoimbra.cpp Fri Jun 16 10:40:53 2017 +0000
@@ -540,4 +540,570 @@
}
return angleBeetweenRobotAndTarget-theta;
}
-*/
\ No newline at end of file
+*/
+
+
+/*
+Lab4
+
+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
+solution:
+(here our sensors are bad but our odometrie s okay
+before each new movement we compute an estimation of where we are, compare it to what the robot think (maybe choose our estimate?)
+*/
+
+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,thetaEstimatedK,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,thetaEstimatedK);
+ this->turn_to_target(PI/2);
+ distanceMoved=0;
+ angleMoved=PI/2;
+ procedure_lab_4(xEstimatedK,yEstimatedK,thetaEstimatedK,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,thetaEstimatedK);
+ }
+}
+
+
+//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 thetaEstimatedKNext=thetaWorldEstimatedK+angleMoved;
+
+ float KRight=0.1;//error constant
+ float KLEft=0.1;//error constant
+ float motionIncrementCovarianceMatrix[2][2];
+ motionIncrementCovarianceMatrix[0][0]=kRight*abs(distanceMovedRight);
+ motionIncrementCovarianceMatrix[0][1]=0;
+ motionIncrementCovarianceMatrix[1][0]=0;
+ motionIncrementCovarianceMatrix[1][1]=kLeft*abs(distanceMovedLeft);
+
+ 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)-(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
+ jacobianFDeltarl[0][1]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
+ jacobianFDeltarl[1][0]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
+ jacobianFDeltarl[1][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)-(distancedMoved/(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)-(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
+ jacobianFDeltarlTranspose[0][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
+ jacobianFDeltarlTranspose[0][2]=1/this->distanceWheels;
+ jacobianFDeltarlTranspose[1][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
+ jacobianFDeltarlTranspose[1][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)-(distancedMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
+ jacobianFDeltarlTranspose[1][2]=-1/this->distanceWheels;
+
+ float tempMatrix3_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){
+ 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 xClosetCellLeft;
+ float yClosetCellLeft;
+ float xClosetCellFront;
+ float yClosetCellFront;
+ float xClosetCellRight;
+ float yClosetCellRight;
+ 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,thetaEstimatedKNext);
+ if((currDistance < this->sonarLeft.maxRange) && currDistance!=-1)
+ //check if distance is lower than previous, update lowest if so
+ if(currDistance < leftCmEstimated){
+ leftCmEstimated= currDistance;
+ xClosetCellLeft=xWorldCell;
+ yClosetCellLeft=yWorldCell;
+ }
+ }
+ //check front
+ currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaEstimatedKNext);
+ if((currDistance < this->sonarFront.maxRange) && currDistance!=-1)
+ //check if distance is lower than previous, update lowest if so
+ if(currDistance < frontCmEstimated){
+ frontCmEstimated= currDistance;
+ xClosetCellFront=xWorldCell;
+ yClosetCellFront=yWorldCell;
+ }
+ }
+ //check right
+ currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaEstimatedKNext);
+ if((currDistance < this->sonarRight.maxRange) && currDistance!=-1)
+ //check if distance is lower than previous, update lowest if so
+ if(currDistance < rightCmEstimated){
+ rightCmEstimated= currDistance;
+ xClosetCellRight=xWorldCell;
+ yClosetCellRight=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(thetaEstimatedKNext)+ySonarLeft*cos(thetaEstimatedKNext)+(yClosestCellLeft-ySonarLeft)*(-xSonarLeft*cos(thetaEstimatedKNext)+ySonarLeft*sin(thetaEstimatedKNext));
+ //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(thetaEstimatedKNext)+ySonarFront*cos(thetaEstimatedKNext)+(yClosestCellFront-ySonarFront)*(-xSonarFront*cos(thetaEstimatedKNext)+ySonarFront*sin(thetaEstimatedKNext));
+ //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(thetaEstimatedKNext)+ySonarRight*cos(thetaEstimatedKNext)+(yClosestCellRight-ySonarRight)*(-xSonarRight*cos(thetaEstimatedKNext)+ySonarRight*sin(thetaEstimatedKNext));
+
+ float innovationCovarianceLeft[3][3];
+ float innovationCovarianceFront[3][3];
+ float innovationCovarianceRight[3][3];
+ //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
+ switch(nbPassed){
+ case 0://none
+ break;
+ case 1://left
+ //compositeMeasurementJacobian = jacobianOfObservationLeft
+ break;
+ case 10://front
+ //compositeMeasurementJacobian = jacobianOfObservationFront
+ break;
+ case 11://left and front
+ float compositeMeasurementJacobian[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
+ float compositeMeasurementJacobian[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
+ float compositeMeasurementJacobian[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
+ float compositeMeasurementJacobian[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
+ switch(nbPassed){
+ //mult nbSonarPassed*3 , 3*3
+ case 0://none
+ break;
+ case 1://left
+ float kalmanGain[3][3];
+ 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
+ float kalmanGain[3][3];
+ 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
+ float kalmanGain[3][3];
+ 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
+ float kalmanGain[3][3];
+ 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
+ float kalmanGain[3][3];
+ 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
+ float kalmanGain[3][3];
+ 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
+ float kalmanGain[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){
+ 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][i][k]/compositeInnovationCovariance;
+ }
+ }
+ //compute kalman gain * composite innovation
+ //mult 3*3 , 3*1
+ float result3_1[3][1];
+ for(i = 0; i < 3; ++i){//mult 3*3, 3*1
+ for(j = 0; j < 1; ++j){
+ for(k = 0; k < 3; ++k){
+ result3_1[i][j] += kalmanGain[i][k] * compositeInnovation[k];
+ }
+ }
+ }
+ //compute updated robot position estimate
+ float xEstimatedKLast=xEstimatedKNext+result3_1[0];
+ float yEstimatedKLast=yEstimatedKNext+result3_1[1];
+ float thetaEstimatedKLast=thetaEstimatedKNext+result3_1[2];
+ //store the resultant covariance for next estimation
+ /*
+ a b c
+ d e f
+ g h i
+
+ a d g
+ b e h
+ c f i
+ */
+ 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;
+ thetaEstimatedK=thetaEstimatedKLast;
+}