test morning
Dependencies: ISR_Mini-explorer mbed
Fork of roboticLab_withclass_3_July by
Diff: MiniExplorerCoimbra.cpp
- Revision:
- 9:1cc27f33d3e1
- Parent:
- 8:072a76960e27
- Child:
- 10:d0109d7cbe7c
--- a/MiniExplorerCoimbra.cpp Mon Jul 10 12:49:07 2017 +0000 +++ b/MiniExplorerCoimbra.cpp Mon Jul 10 16:23:52 2017 +0000 @@ -3,7 +3,7 @@ #define PI 3.14159 -MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld, float widthRealMap, float heightRealMap):map(widthRealMap,heightRealMap,12,8),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,12,8),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4),covariancePositionEstimationK(3,3){ i2c1.frequency(100000); initRobot(); //Initializing the robot pc.baud(9600); // baud for the pc communication @@ -31,24 +31,9 @@ this->speed=300; this->rangeForce=50; - //not too bad values 200 and -40000 - //not too bad values 200 and -20000 - //not too bad values 500 and -20000 - //not too bad values 500 and -25000 rangeForce 50 this->attractionConstantForce=1; - this->repulsionConstantForce=-90;//-90 ok //idea make repulsion less divaded by distance - - this->covariancePositionEstimationK[0][0]=0; - this->covariancePositionEstimationK[0][1]=0; - this->covariancePositionEstimationK[0][2]=0; - - this->covariancePositionEstimationK[1][0]=0; - this->covariancePositionEstimationK[1][1]=0; - this->covariancePositionEstimationK[1][2]=0; - - this->covariancePositionEstimationK[2][0]=0; - this->covariancePositionEstimationK[2][1]=0; - this->covariancePositionEstimationK[2][2]=0; + //-90 ok with attraction impacted by distance + this->repulsionConstantForce=-90; } @@ -594,6 +579,11 @@ //this->print_map_with_robot_position(); //pc.printf("\r\nForce X repul:%f",*forceXWorld); //pc.printf("\r\nForce Y repul:%f",*forceYWorld); + + //test without atraction being impacted by distance + //*forceXWorld+=this->attractionConstantForce*(targetXWorld-this->xWorld); + //*forceYWorld+=this->attractionConstantForce*(targetYWorld-this->yWorld); + float distanceTargetRobot=sqrt(pow(targetXWorld-this->xWorld,2)+pow(targetYWorld-this->yWorld,2)); if(distanceTargetRobot != 0){ *forceXWorld+=this->attractionConstantForce*(targetXWorld-this->xWorld)/distanceTargetRobot; @@ -602,6 +592,7 @@ *forceXWorld+=this->attractionConstantForce*(targetXWorld-this->xWorld)/0.001; *forceYWorld+=this->attractionConstantForce*(targetYWorld-this->yWorld)/0.001; } + //pc.printf("\r\nForce X after attract:%f",*forceXWorld); //pc.printf("\r\nForce Y after attract:%f",*forceYWorld); @@ -867,44 +858,7 @@ //move of targetXWorld and targetYWorld ending in a targetAngleWorld void MiniExplorerCoimbra::go_to_point_with_angle_kalman(float targetXWorld, float targetYWorld, float targetAngleWorld) { - float dt; - Timer t; - float distanceToTarget; - //make sure the target is correct - if(targetAngleWorld > PI) - targetAngleWorld=-2*PI+targetAngleWorld; - if(targetAngleWorld < -PI) - targetAngleWorld=2*PI+targetAngleWorld; - - do { - //Timer stuff - dt = t.read(); - t.reset(); - t.start(); - - //Updating X,Y and theta with the odometry values - this->OdometriaKalmanFilter(); - - //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\rdist to target= %f",distanceToTarget); - - } while(distanceToTarget>10 || (abs(targetAngleWorld-this->thetaWorld)>0.1)); - - //Stop at the end - leftMotor(1,0); - rightMotor(1,0); - pc.printf("\r\nReached Target!"); -} - -void MiniExplorerCoimbra::test_prediction_sonar(){ - this->map.fill_map_with_kalman_knowledge(); - - //=====KINEMATICS=========================== + //=====KINEMATICS=========================== float R_cm; float L_cm; @@ -924,55 +878,67 @@ float distanceMovedY=distanceMoved*sin(this->thetaWorld+angleMoved/2); //try with world coordinate system - - float xEstimatedK=this->xWorld+distanceMovedX; - float yEstimatedK=this->yWorld+distanceMovedY; - float thetaWorldEstimatedK = this->thetaWorld+angleMoved; - - pc.printf("\r\n X=%f, Y=%f, theta=%f",xEstimatedK,yEstimatedK,thetaWorldEstimatedK); + myMatrix poseKplus1K(3,1); + poseKplus1K.data[0][0]=this->xWorld+distanceMovedX; + poseKplus1K.data[1][0]=this->yWorld+distanceMovedY; + poseKplus1K.data[2][0]=this->thetaWorld+angleMoved; - //try with robot coordinate system - /* - float xEstimatedK=X; - float yEstimatedK=Y; - float thetaWorldEstimatedK = theta; - */ + pc.printf("\r\n X=%f, Y=%f, theta=%f",poseKplus1K.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]); + //=====ERROR_MODEL=========================== //FP Matrix - float Fp[3][3]={{1,0,0},{0,1,0},{0,0,1}}; - - Fp[0][2]=-1*distanceMoved*sin(this->thetaWorld+(angleMoved/2)); - Fp[1][2]=distanceMoved*cos(this->thetaWorld+(angleMoved/2)); - + myMatrix Fp(3,3); + Fp.data[0][0]=1; + Fp.data[1][1]=1; + Fp.data[2][2]=1; + Fp.data[0][2]=-1*distanceMoved*sin(this->thetaWorld+(angleMoved/2)); + Fp.data[1][2]=distanceMoved*cos(this->thetaWorld+(angleMoved/2)); + + myMatrix FpTranspose(3,3); + FpTranspose.fillWithTranspose(Fp); + //Frl matrix - float Frl[3][2]={{0,0},{0,0},{(1/this->distanceWheels),-(1/this->distanceWheels)}}; - - Frl[0][0]=0.5*cos(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2)); - Frl[0][1]=0.5*cos(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2)); - Frl[1][0]=0.5*sin(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2)); - Frl[1][1]=0.5*sin(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2)); - + myMatrix Frl(3,2); + Frl.data[0][0]=0.5*cos(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2)); + Frl.data[0][1]=0.5*cos(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2)); + Frl.data[1][0]=0.5*sin(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2)); + Frl.data[1][1]=0.5*sin(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2)); + Frl.data[2][0]=(1/this->distanceWheels); + Frl.data[2][1]=-(1/this->distanceWheels) ; + + myMatrix FrlTranspose(2,3); + FrlTranspose.fillWithTranspose(Frl); + //error constants... float kr=1; float kl=1; - float covar[2][2]={{kr*abs(R_cm), 0}, {0, kl*abs(L_cm)}}; + myMatrix covar(2,2); + covar.data[0][0]=kr*abs(R_cm); + covar.data[1][1]=kl*abs(L_cm); //uncertainty positionx, and theta at //1,1,1 - float Q[3][3]={{1,0,0}, {0,2,0}, {0,0,0.01}}; - - covariancePositionEstimationK[0][0]=covar[0][0]*pow(Frl[0][0],2)+covar[1][1]*pow(Frl[0][1],2)+covariancePositionEstimationK[0][0]+Q[0][0]+covariancePositionEstimationK[2][0]*Fp[0][2]+Fp[0][2]*(covariancePositionEstimationK[0][2]+covariancePositionEstimationK[2][2]*Fp[0][2]); - covariancePositionEstimationK[0][1]=covariancePositionEstimationK[0][1]+covariancePositionEstimationK[2][1]*Fp[0][2]+Fp[1][2]*(covariancePositionEstimationK[0][2]+covariancePositionEstimationK[2][2]*Fp[0][2])+covar[0][0]*Frl[0][0]*Frl[1][0]+covar[1][1]*Frl[0][1]; - covariancePositionEstimationK[0][2]=covariancePositionEstimationK[0][2]+covariancePositionEstimationK[2][2]*Fp[0][2]+covar[0][0]*Frl[0][0]*Frl[2][0]+covar[1][1]*Frl[0][1]*Frl[2][1]; - covariancePositionEstimationK[1][0]=covariancePositionEstimationK[1][0]+covariancePositionEstimationK[2][0]*Fp[1][2]+Fp[0][2]*(covariancePositionEstimationK[1][2]+covariancePositionEstimationK[2][2]*Fp[1][2])+covar[0][0]*Frl[0][0]*Frl[1][0]+covar[1][1]*Frl[0][1]*Frl[1][1]; - covariancePositionEstimationK[1][1]=covar[0][0]*pow(Frl[1][0],2)+covar[1][1]*pow(Frl[1][1],2)+covariancePositionEstimationK[1][1]+Q[1][1]+covariancePositionEstimationK[2][1]*Fp[1][2]+Fp[1][2]*(covariancePositionEstimationK[1][2]+covariancePositionEstimationK[2][2]*Fp[1][2]); - covariancePositionEstimationK[1][2]=covariancePositionEstimationK[1][2]+covariancePositionEstimationK[2][2]*Fp[1][2]+covar[0][0]*Frl[1][0]*Frl[2][0]+covar[1][1]*Frl[1][1]*Frl[2][1]; - covariancePositionEstimationK[2][0]=covariancePositionEstimationK[2][0]+covariancePositionEstimationK[2][2]*Fp[0][2]+covar[0][0]*Frl[0][0]*Frl[2][0]+covar[1][1]*Frl[0][1]*Frl[2][1]; - covariancePositionEstimationK[2][1]=covariancePositionEstimationK[2][1]+covariancePositionEstimationK[2][2]*Fp[1][2]+covar[0][0]*Frl[1][0]*Frl[2][0]+covar[1][1]*Frl[1][1]*Frl[2][1]; - covariancePositionEstimationK[2][2]=covar[0][0]*pow(Frl[2][1],2)+covar[1][1]*pow(Frl[2][1],2)+covariancePositionEstimationK[2][2]+Q[2][2]; - - + myMatrix Q(3,3); + Q.data[0][0]=1; + Q.data[1][1]=2; + Q.data[2][2]=0.01; + + //new covariance= Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose +Q + myMatrix covariancePositionEstimationKplus1K(3,3); + myMatrix temp1(3,3); + temp1.fillByMultiplication(Fp,this->covariancePositionEstimationK);//Fp*old covariance + myMatrix temp2(3,3); + temp2.fillByMultiplication(temp1,FpTranspose);//Fp*old covariance*FpTranspose + temp1.fillWithZeroes(); + myMatrix temp3(3,2); + temp3.fillByMultiplication(Frl,covar);//Frl*covar + temp1.fillByMultiplication(temp3,FrlTranspose);//Frl*covar*FrlTranspose + + covariancePositionEstimationKplus1K.addition(temp2);//Fp*old covariance*FpTranspose + covariancePositionEstimationKplus1K.addition(temp1);//Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose + covariancePositionEstimationKplus1K.addition(Q);//Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose +Q + //=====OBSERVATION===== //get the estimated measure we should have according to our knowledge of the map and the previously computed localisation @@ -997,7 +963,7 @@ 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,xEstimatedK,yEstimatedK,thetaWorldEstimatedK); + currDistance=this->sonarLeft.isInRange(xWorldCell,yWorldCell,poseKplus1K.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]); if((currDistance < this->sonarLeft.maxRange) && currDistance > -1){ //check if distance is lower than previous, update lowest if so if(currDistance < leftCmEstimated){ @@ -1007,7 +973,7 @@ } } //check front - currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,xEstimatedK,yEstimatedK,thetaWorldEstimatedK); + currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,poseKplus1K.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]); if((currDistance < this->sonarFront.maxRange) && currDistance > -1){ //check if distance is lower than previous, update lowest if so if(currDistance < frontCmEstimated){ @@ -1017,7 +983,7 @@ } } //check right - currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,xEstimatedK,yEstimatedK,thetaWorldEstimatedK); + currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,poseKplus1K.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]); if((currDistance < this->sonarRight.maxRange) && currDistance > -1){ //check if distance is lower than previous, update lowest if so if(currDistance < rightCmEstimated){ @@ -1052,28 +1018,37 @@ float innovationRight=rightCm-rightCmEstimated; //compute jacobian of observation - float jacobianOfObservationLeft[1][3]; - float jacobianOfObservationRight[1][3]; - float jacobianOfObservationFront[1][3]; - float xSonarLeft=xEstimatedK+this->sonarLeft.distanceX; - float ySonarLeft=yEstimatedK+this->sonarLeft.distanceY; + myMatrix jacobianOfObservationLeft(1,3); + myMatrix jacobianOfObservationRight(1,3); + myMatrix jacobianOfObservationFront(1,3); + + float xSonarLeft=poseKplus1K.data[0][0]+this->sonarLeft.distanceX; + float ySonarLeft=poseKplus1K.data[1][0]+this->sonarLeft.distanceY; //left - jacobianOfObservationLeft[0][0]=(xSonarLeft-xClosestCellLeft)/leftCmEstimated; - jacobianOfObservationLeft[0][1]=(ySonarLeft-yClosestCellLeft)/leftCmEstimated; - jacobianOfObservationLeft[0][2]=(xClosestCellLeft-xSonarLeft)*(xSonarLeft*sin(thetaWorldEstimatedK)+ySonarLeft*cos(thetaWorldEstimatedK))+(yClosestCellLeft-ySonarLeft)*(-xSonarLeft*cos(thetaWorldEstimatedK)+ySonarLeft*sin(thetaWorldEstimatedK)); + jacobianOfObservationLeft.data[0][0]=(xSonarLeft-xClosestCellLeft)/leftCmEstimated; + jacobianOfObservationLeft.data[0][1]=(ySonarLeft-yClosestCellLeft)/leftCmEstimated; + jacobianOfObservationLeft.data[0][2]=(xClosestCellLeft-xSonarLeft)*(xSonarLeft*sin(poseKplus1K.data[2][0])+ySonarLeft*cos(poseKplus1K.data[2][0]))+(yClosestCellLeft-ySonarLeft)*(-xSonarLeft*cos(poseKplus1K.data[2][0])+ySonarLeft*sin(poseKplus1K.data[2][0])); //front - float xSonarFront=xEstimatedK+this->sonarFront.distanceX; - float ySonarFront=yEstimatedK+this->sonarFront.distanceY; - jacobianOfObservationFront[0][0]=(xSonarFront-xClosestCellFront)/frontCmEstimated; - jacobianOfObservationFront[0][1]=(ySonarFront-yClosestCellFront)/frontCmEstimated; - jacobianOfObservationFront[0][2]=(xClosestCellFront-xSonarFront)*(xSonarFront*sin(thetaWorldEstimatedK)+ySonarFront*cos(thetaWorldEstimatedK))+(yClosestCellFront-ySonarFront)*(-xSonarFront*cos(thetaWorldEstimatedK)+ySonarFront*sin(thetaWorldEstimatedK)); + float xSonarFront=poseKplus1K.data[0][0]+this->sonarFront.distanceX; + float ySonarFront=poseKplus1K.data[1][0]+this->sonarFront.distanceY; + jacobianOfObservationFront.data[0][0]=(xSonarFront-xClosestCellFront)/frontCmEstimated; + jacobianOfObservationFront.data[0][1]=(ySonarFront-yClosestCellFront)/frontCmEstimated; + jacobianOfObservationFront.data[0][2]=(xClosestCellFront-xSonarFront)*(xSonarFront*sin(poseKplus1K.data[2][0])+ySonarFront*cos(poseKplus1K.data[2][0]))+(yClosestCellFront-ySonarFront)*(-xSonarFront*cos(poseKplus1K.data[2][0])+ySonarFront*sin(poseKplus1K.data[2][0])); //right - float xSonarRight=xEstimatedK+this->sonarRight.distanceX; - float ySonarRight=yEstimatedK+this->sonarRight.distanceY; - jacobianOfObservationRight[0][0]=(xSonarRight-xClosestCellRight)/rightCmEstimated; - jacobianOfObservationRight[0][1]=(ySonarRight-yClosestCellRight)/rightCmEstimated; - jacobianOfObservationRight[0][2]=(xClosestCellRight-xSonarRight)*(xSonarRight*sin(thetaWorldEstimatedK)+ySonarRight*cos(thetaWorldEstimatedK))+(yClosestCellRight-ySonarRight)*(-xSonarRight*cos(thetaWorldEstimatedK)+ySonarRight*sin(thetaWorldEstimatedK)); - + float xSonarRight=poseKplus1K.data[0][0]+this->sonarRight.distanceX; + float ySonarRight=poseKplus1K.data[1][0]+this->sonarRight.distanceY; + jacobianOfObservationRight.data[0][0]=(xSonarRight-xClosestCellRight)/rightCmEstimated; + jacobianOfObservationRight.data[0][1]=(ySonarRight-yClosestCellRight)/rightCmEstimated; + jacobianOfObservationRight.data[0][2]=(xClosestCellRight-xSonarRight)*(xSonarRight*sin(poseKplus1K.data[2][0])+ySonarRight*cos(poseKplus1K.data[2][0]))+(yClosestCellRight-ySonarRight)*(-xSonarRight*cos(poseKplus1K.data[2][0])+ySonarRight*sin(poseKplus1K.data[2][0])); + + myMatrix jacobianOfObservationRightTranspose(3,1); + jacobianOfObservationRightTranspose.fillWithTranspose(jacobianOfObservationRight); + + myMatrix jacobianOfObservationFrontTranspose(3,1); + jacobianOfObservationFrontTranspose.fillWithTranspose(jacobianOfObservationFront); + + myMatrix jacobianOfObservationLeftTranspose(3,1); + jacobianOfObservationLeftTranspose.fillWithTranspose(jacobianOfObservationLeft); //error constants 0,0,0 sonars perfect; must be found by experimenting; gives mean and standanrt deviation //let's assume //in centimeters @@ -1084,9 +1059,26 @@ //R-> 4 in diagonal //S for each sonar (concatenated covariance matrix of innovation) - float innovationCovarianceFront=R_front+ jacobianOfObservationFront[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]); - float innovationCovarianceLeft=R_left + jacobianOfObservationLeft[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]); - float innovationCovarianceRight=R_right+ jacobianOfObservationRight[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]); + //equ (12), innovationCovariance =JacobianObservation*covariancePositionEstimationKplus1K*JacobianObservationTranspose+R + myMatrix temp4(1,3); + temp4.fillByMultiplication(jacobianOfObservationFront,covariancePositionEstimationKplus1K); + myMatrix temp5(1,1); + temp5.fillByMultiplication(temp4,jacobianOfObservationFrontTranspose); + float innovationCovarianceFront=temp5.data[0][0]+R_front; + temp4.fillWithZeroes(); + temp5.fillWithZeroes(); + + + temp4.fillByMultiplication(jacobianOfObservationLeft,covariancePositionEstimationKplus1K); + temp5.fillByMultiplication(temp4,jacobianOfObservationLeftTranspose); + float innovationCovarianceLeft=temp5.data[0][0]+R_left; + temp4.fillWithZeroes(); + temp5.fillWithZeroes(); + + + temp4.fillByMultiplication(jacobianOfObservationRight,covariancePositionEstimationKplus1K); + temp5.fillByMultiplication(temp4,jacobianOfObservationRightTranspose); + float innovationCovarianceRight=temp5.data[0][0]+R_right; //check if it pass the validation gate float gateScoreLeft=innovationLeft*innovationLeft/innovationCovarianceLeft; @@ -1142,55 +1134,68 @@ float distanceMovedY=distanceMoved*sin(this->thetaWorld+angleMoved/2); //try with world coordinate system - - float xEstimatedK=this->xWorld+distanceMovedX; - float yEstimatedK=this->yWorld+distanceMovedY; - float thetaWorldEstimatedK = this->thetaWorld+angleMoved; - - pc.printf("\r\n X=%f, Y=%f, theta=%f",xEstimatedK,yEstimatedK,thetaWorldEstimatedK); + myMatrix poseKplus1K(3,1); + poseKplus1K.data[0][0]=this->xWorld+distanceMovedX; + poseKplus1K.data[1][0]=this->yWorld+distanceMovedY; + poseKplus1K.data[2][0]=this->thetaWorld+angleMoved; - //try with robot coordinate system - /* - float xEstimatedK=X; - float yEstimatedK=Y; - float thetaWorldEstimatedK = theta; - */ + pc.printf("\r\n X=%f, Y=%f, theta=%f",poseKplus1K.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]); + //=====ERROR_MODEL=========================== //FP Matrix - float Fp[3][3]={{1,0,0},{0,1,0},{0,0,1}}; - - Fp[0][2]=-1*distanceMoved*sin(this->thetaWorld+(angleMoved/2)); - Fp[1][2]=distanceMoved*cos(this->thetaWorld+(angleMoved/2)); - + myMatrix Fp(3,3); + Fp.data[0][0]=1; + Fp.data[1][1]=1; + Fp.data[2][2]=1; + Fp.data[0][2]=-1*distanceMoved*sin(this->thetaWorld+(angleMoved/2)); + Fp.data[1][2]=distanceMoved*cos(this->thetaWorld+(angleMoved/2)); + + myMatrix FpTranspose(3,3); + FpTranspose.fillWithTranspose(Fp); + //Frl matrix - float Frl[3][2]={{0,0},{0,0},{(1/this->distanceWheels),-(1/this->distanceWheels)}}; - - Frl[0][0]=0.5*cos(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2)); - Frl[0][1]=0.5*cos(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2)); - Frl[1][0]=0.5*sin(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2)); - Frl[1][1]=0.5*sin(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2)); - + myMatrix Frl(3,2); + Frl.data[0][0]=0.5*cos(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2)); + Frl.data[0][1]=0.5*cos(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*sin(this->thetaWorld+(angleMoved/2)); + Frl.data[1][0]=0.5*sin(this->thetaWorld+(angleMoved/2))+(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2)); + Frl.data[1][1]=0.5*sin(this->thetaWorld+(angleMoved/2))-(distanceMoved/(2*this->distanceWheels))*cos(this->thetaWorld+(angleMoved/2)); + Frl.data[2][0]=(1/this->distanceWheels); + Frl.data[2][1]=-(1/this->distanceWheels) ; + + myMatrix FrlTranspose(2,3); + FrlTranspose.fillWithTranspose(Frl); + //error constants... float kr=1; float kl=1; - float covar[2][2]={{kr*abs(R_cm), 0}, {0, kl*abs(L_cm)}}; + myMatrix covar(2,2); + covar.data[0][0]=kr*abs(R_cm); + covar.data[1][1]=kl*abs(L_cm); //uncertainty positionx, and theta at //1,1,1 - float Q[3][3]={{1,0,0}, {0,2,0}, {0,0,0.01}}; - - covariancePositionEstimationK[0][0]=covar[0][0]*pow(Frl[0][0],2)+covar[1][1]*pow(Frl[0][1],2)+covariancePositionEstimationK[0][0]+Q[0][0]+covariancePositionEstimationK[2][0]*Fp[0][2]+Fp[0][2]*(covariancePositionEstimationK[0][2]+covariancePositionEstimationK[2][2]*Fp[0][2]); - covariancePositionEstimationK[0][1]=covariancePositionEstimationK[0][1]+covariancePositionEstimationK[2][1]*Fp[0][2]+Fp[1][2]*(covariancePositionEstimationK[0][2]+covariancePositionEstimationK[2][2]*Fp[0][2])+covar[0][0]*Frl[0][0]*Frl[1][0]+covar[1][1]*Frl[0][1]; - covariancePositionEstimationK[0][2]=covariancePositionEstimationK[0][2]+covariancePositionEstimationK[2][2]*Fp[0][2]+covar[0][0]*Frl[0][0]*Frl[2][0]+covar[1][1]*Frl[0][1]*Frl[2][1]; - covariancePositionEstimationK[1][0]=covariancePositionEstimationK[1][0]+covariancePositionEstimationK[2][0]*Fp[1][2]+Fp[0][2]*(covariancePositionEstimationK[1][2]+covariancePositionEstimationK[2][2]*Fp[1][2])+covar[0][0]*Frl[0][0]*Frl[1][0]+covar[1][1]*Frl[0][1]*Frl[1][1]; - covariancePositionEstimationK[1][1]=covar[0][0]*pow(Frl[1][0],2)+covar[1][1]*pow(Frl[1][1],2)+covariancePositionEstimationK[1][1]+Q[1][1]+covariancePositionEstimationK[2][1]*Fp[1][2]+Fp[1][2]*(covariancePositionEstimationK[1][2]+covariancePositionEstimationK[2][2]*Fp[1][2]); - covariancePositionEstimationK[1][2]=covariancePositionEstimationK[1][2]+covariancePositionEstimationK[2][2]*Fp[1][2]+covar[0][0]*Frl[1][0]*Frl[2][0]+covar[1][1]*Frl[1][1]*Frl[2][1]; - covariancePositionEstimationK[2][0]=covariancePositionEstimationK[2][0]+covariancePositionEstimationK[2][2]*Fp[0][2]+covar[0][0]*Frl[0][0]*Frl[2][0]+covar[1][1]*Frl[0][1]*Frl[2][1]; - covariancePositionEstimationK[2][1]=covariancePositionEstimationK[2][1]+covariancePositionEstimationK[2][2]*Fp[1][2]+covar[0][0]*Frl[1][0]*Frl[2][0]+covar[1][1]*Frl[1][1]*Frl[2][1]; - covariancePositionEstimationK[2][2]=covar[0][0]*pow(Frl[2][1],2)+covar[1][1]*pow(Frl[2][1],2)+covariancePositionEstimationK[2][2]+Q[2][2]; - - //=====OBSERVATION===== + myMatrix Q(3,3); + Q.data[0][0]=1; + Q.data[1][1]=2; + Q.data[2][2]=0.01; + + //new covariance= Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose +Q + myMatrix covariancePositionEstimationKplus1K(3,3); + myMatrix temp1(3,3); + temp1.fillByMultiplication(Fp,this->covariancePositionEstimationK);//Fp*old covariance + myMatrix temp2(3,3); + temp2.fillByMultiplication(temp1,FpTranspose);//Fp*old covariance*FpTranspose + temp1.fillWithZeroes(); + myMatrix temp3(3,2); + temp3.fillByMultiplication(Frl,covar);//Frl*covar + temp1.fillByMultiplication(temp3,FrlTranspose);//Frl*covar*FrlTranspose + + covariancePositionEstimationKplus1K.addition(temp2);//Fp*old covariance*FpTranspose + covariancePositionEstimationKplus1K.addition(temp1);//Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose + covariancePositionEstimationKplus1K.addition(Q);//Fp*old covariance*FpTranspose +Frl*covar*FrlTranspose +Q + + //=====OBSERVATION===== //get the estimated measure we should have according to our knowledge of the map and the previously computed localisation float leftCmEstimated=this->sonarLeft.maxRange; @@ -1209,12 +1214,12 @@ 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)<0.5){ + if(this->map.get_proba_cell(i,j)>0.5){ //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,xEstimatedK,yEstimatedK,thetaWorldEstimatedK); + currDistance=this->sonarLeft.isInRange(xWorldCell,yWorldCell,poseKplus1K.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]); if((currDistance < this->sonarLeft.maxRange) && currDistance > -1){ //check if distance is lower than previous, update lowest if so if(currDistance < leftCmEstimated){ @@ -1224,7 +1229,7 @@ } } //check front - currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,xEstimatedK,yEstimatedK,thetaWorldEstimatedK); + currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,poseKplus1K.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]); if((currDistance < this->sonarFront.maxRange) && currDistance > -1){ //check if distance is lower than previous, update lowest if so if(currDistance < frontCmEstimated){ @@ -1234,7 +1239,7 @@ } } //check right - currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,xEstimatedK,yEstimatedK,thetaWorldEstimatedK); + currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,poseKplus1K.data[0][0],poseKplus1K.data[1][0],poseKplus1K.data[2][0]); if((currDistance < this->sonarRight.maxRange) && currDistance > -1){ //check if distance is lower than previous, update lowest if so if(currDistance < rightCmEstimated){ @@ -1251,6 +1256,9 @@ float leftCm = get_distance_left_sensor()/10; float frontCm = get_distance_front_sensor()/10; float rightCm = get_distance_right_sensor()/10; + + pc.printf("\r\n1: Lcm=%f, FCm=%f, RCm=%f",leftCm,frontCm,rightCm); + //if superior to sonar range, put the value to sonar max range + 1 if(leftCm > this->sonarLeft.maxRange) leftCm=this->sonarLeft.maxRange; @@ -1263,30 +1271,40 @@ //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; + float innovationRight=rightCm-rightCmEstimated; + //compute jacobian of observation - float jacobianOfObservationLeft[1][3]; - float jacobianOfObservationRight[1][3]; - float jacobianOfObservationFront[1][3]; - float xSonarLeft=xEstimatedK+this->sonarLeft.distanceX; - float ySonarLeft=yEstimatedK+this->sonarLeft.distanceY; + myMatrix jacobianOfObservationLeft(1,3); + myMatrix jacobianOfObservationRight(1,3); + myMatrix jacobianOfObservationFront(1,3); + + float xSonarLeft=poseKplus1K.data[0][0]+this->sonarLeft.distanceX; + float ySonarLeft=poseKplus1K.data[1][0]+this->sonarLeft.distanceY; //left - jacobianOfObservationLeft[0][0]=(xSonarLeft-xClosestCellLeft)/leftCmEstimated; - jacobianOfObservationLeft[0][1]=(ySonarLeft-yClosestCellLeft)/leftCmEstimated; - jacobianOfObservationLeft[0][2]=(xClosestCellLeft-xSonarLeft)*(xSonarLeft*sin(thetaWorldEstimatedK)+ySonarLeft*cos(thetaWorldEstimatedK))+(yClosestCellLeft-ySonarLeft)*(-xSonarLeft*cos(thetaWorldEstimatedK)+ySonarLeft*sin(thetaWorldEstimatedK)); + jacobianOfObservationLeft.data[0][0]=(xSonarLeft-xClosestCellLeft)/leftCmEstimated; + jacobianOfObservationLeft.data[0][1]=(ySonarLeft-yClosestCellLeft)/leftCmEstimated; + jacobianOfObservationLeft.data[0][2]=(xClosestCellLeft-xSonarLeft)*(xSonarLeft*sin(poseKplus1K.data[2][0])+ySonarLeft*cos(poseKplus1K.data[2][0]))+(yClosestCellLeft-ySonarLeft)*(-xSonarLeft*cos(poseKplus1K.data[2][0])+ySonarLeft*sin(poseKplus1K.data[2][0])); //front - float xSonarFront=xEstimatedK+this->sonarFront.distanceX; - float ySonarFront=yEstimatedK+this->sonarFront.distanceY; - jacobianOfObservationFront[0][0]=(xSonarFront-xClosestCellFront)/frontCmEstimated; - jacobianOfObservationFront[0][1]=(ySonarFront-yClosestCellFront)/frontCmEstimated; - jacobianOfObservationFront[0][2]=(xClosestCellFront-xSonarFront)*(xSonarFront*sin(thetaWorldEstimatedK)+ySonarFront*cos(thetaWorldEstimatedK))+(yClosestCellFront-ySonarFront)*(-xSonarFront*cos(thetaWorldEstimatedK)+ySonarFront*sin(thetaWorldEstimatedK)); + float xSonarFront=poseKplus1K.data[0][0]+this->sonarFront.distanceX; + float ySonarFront=poseKplus1K.data[1][0]+this->sonarFront.distanceY; + jacobianOfObservationFront.data[0][0]=(xSonarFront-xClosestCellFront)/frontCmEstimated; + jacobianOfObservationFront.data[0][1]=(ySonarFront-yClosestCellFront)/frontCmEstimated; + jacobianOfObservationFront.data[0][2]=(xClosestCellFront-xSonarFront)*(xSonarFront*sin(poseKplus1K.data[2][0])+ySonarFront*cos(poseKplus1K.data[2][0]))+(yClosestCellFront-ySonarFront)*(-xSonarFront*cos(poseKplus1K.data[2][0])+ySonarFront*sin(poseKplus1K.data[2][0])); //right - float xSonarRight=xEstimatedK+this->sonarRight.distanceX; - float ySonarRight=yEstimatedK+this->sonarRight.distanceY; - jacobianOfObservationRight[0][0]=(xSonarRight-xClosestCellRight)/rightCmEstimated; - jacobianOfObservationRight[0][1]=(ySonarRight-yClosestCellRight)/rightCmEstimated; - jacobianOfObservationRight[0][2]=(xClosestCellRight-xSonarRight)*(xSonarRight*sin(thetaWorldEstimatedK)+ySonarRight*cos(thetaWorldEstimatedK))+(yClosestCellRight-ySonarRight)*(-xSonarRight*cos(thetaWorldEstimatedK)+ySonarRight*sin(thetaWorldEstimatedK)); - + float xSonarRight=poseKplus1K.data[0][0]+this->sonarRight.distanceX; + float ySonarRight=poseKplus1K.data[1][0]+this->sonarRight.distanceY; + jacobianOfObservationRight.data[0][0]=(xSonarRight-xClosestCellRight)/rightCmEstimated; + jacobianOfObservationRight.data[0][1]=(ySonarRight-yClosestCellRight)/rightCmEstimated; + jacobianOfObservationRight.data[0][2]=(xClosestCellRight-xSonarRight)*(xSonarRight*sin(poseKplus1K.data[2][0])+ySonarRight*cos(poseKplus1K.data[2][0]))+(yClosestCellRight-ySonarRight)*(-xSonarRight*cos(poseKplus1K.data[2][0])+ySonarRight*sin(poseKplus1K.data[2][0])); + + myMatrix jacobianOfObservationRightTranspose(3,1); + jacobianOfObservationRightTranspose.fillWithTranspose(jacobianOfObservationRight); + + myMatrix jacobianOfObservationFrontTranspose(3,1); + jacobianOfObservationFrontTranspose.fillWithTranspose(jacobianOfObservationFront); + + myMatrix jacobianOfObservationLeftTranspose(3,1); + jacobianOfObservationLeftTranspose.fillWithTranspose(jacobianOfObservationLeft); //error constants 0,0,0 sonars perfect; must be found by experimenting; gives mean and standanrt deviation //let's assume //in centimeters @@ -1297,9 +1315,26 @@ //R-> 4 in diagonal //S for each sonar (concatenated covariance matrix of innovation) - float innovationCovarianceFront=R_front+ jacobianOfObservationFront[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]); - float innovationCovarianceLeft=R_left+ jacobianOfObservationLeft[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]); - float innovationCovarianceRight=R_right+ jacobianOfObservationRight[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]); + //equ (12), innovationCovariance =JacobianObservation*covariancePositionEstimationKplus1K*JacobianObservationTranspose+R + myMatrix temp4(1,3); + temp4.fillByMultiplication(jacobianOfObservationFront,covariancePositionEstimationKplus1K); + myMatrix temp5(1,1); + temp5.fillByMultiplication(temp4,jacobianOfObservationFrontTranspose); + float innovationCovarianceFront=temp5.data[0][0]+R_front; + temp4.fillWithZeroes(); + temp5.fillWithZeroes(); + + + temp4.fillByMultiplication(jacobianOfObservationLeft,covariancePositionEstimationKplus1K); + temp5.fillByMultiplication(temp4,jacobianOfObservationLeftTranspose); + float innovationCovarianceLeft=temp5.data[0][0]+R_left; + temp4.fillWithZeroes(); + temp5.fillWithZeroes(); + + + temp4.fillByMultiplication(jacobianOfObservationRight,covariancePositionEstimationKplus1K); + temp5.fillByMultiplication(temp4,jacobianOfObservationRightTranspose); + float innovationCovarianceRight=temp5.data[0][0]+R_right; //check if it pass the validation gate float gateScoreLeft=innovationLeft*innovationLeft/innovationCovarianceLeft; @@ -1309,7 +1344,6 @@ int frontPassed=0; int rightPassed=0; - pc.printf("\r\n Gate scre L=%f, F=%f, R=%f",gateScoreLeft,gateScoreFront,gateScoreRight); //5cm -> 25 int errorsquare=25;//constant error @@ -1322,229 +1356,312 @@ //for those who passed //compute composite innovation int nbPassed=leftPassed+frontPassed+rightPassed; - - float xEstimatedKNext=xEstimatedK; - float yEstimatedKNext=xEstimatedK; - float thetaWorldEstimatedKNext=thetaWorldEstimatedK; - - float compositeInnovationCovariance3x3[3][3]={{0,0,0}, {0,0,0}, {0,0,0}}; - - float compositeInnovationCovariance2x2[2][2]={{0,0}, {0,0}}; - - float compositeInnovationCovariance1x1=0; - - float kalmanGain3X1[3][1]={{0}, {0}, {0}}; - float kalmanGain3X2[3][2]={{0,0}, {0.0}, {0,0}}; - float kalmanGain3X3[3][3]={{0,0,0}, {0,0,0}, {0,0,0}}; - + + //compositeMeasurementJacobian + myMatrix compositeMeasurementJacobian1x3(1,3); + myMatrix compositeMeasurementJacobian2x3(2,3); + myMatrix compositeMeasurementJacobian3x3(3,3); + + myMatrix compositeMeasurementJacobian1x3Transpose(3,1); + myMatrix compositeMeasurementJacobian2x3Transpose(3,2); + myMatrix compositeMeasurementJacobian3x3Transpose(3,3); + + //compositeInnovation + myMatrix compositeInnovation3x1(3,1); + myMatrix compositeInnovation2x1(2,1); + myMatrix compositeInnovation1x1(1,1); + + //compositeInnovationCovariance + myMatrix compositeInnovationCovariance3x3(3,3); + myMatrix compositeInnovationCovariance2x2(2,2); + myMatrix compositeInnovationCovariance1x1(1,1); + + myMatrix tempCompositeInnovationCovariance3x3(3,3); + myMatrix tempCompositeInnovationCovariance2x2(2,2); + + myMatrix compositeInnovationCovariance3x3Inverse(3,3); + myMatrix compositeInnovationCovariance2x2Inverse(2,2); + myMatrix compositeInnovationCovariance1x1Inverse(1,1); + + //Kalman Gain + myMatrix kalmanGain3X3(3,3); + myMatrix kalmanGain3X2(3,2); + myMatrix kalmanGain3X1(3,1); + + myMatrix tempKalmanGain3X3(3,3); + myMatrix tempKalmanGain3X2(3,2); + myMatrix tempKalmanGain3X1(3,1); + + myMatrix kalmanGain3X3Transpose(3,3); + myMatrix kalmanGain3X2Transpose(2,3); + myMatrix kalmanGain3X1Transpose(1,3); + + //new pose estimation + myMatrix poseKplus1Kplus1(3,1); + poseKplus1Kplus1.fillByCopy(poseKplus1Kplus1); + myMatrix tempPoseKplus1Kplus1(3,1); + + //covariancePositionEstimationKplus1Kplus1 + myMatrix covariancePositionEstimationKplus1Kplus1(3,3); + covariancePositionEstimationKplus1Kplus1.fillByCopy(covariancePositionEstimationKplus1K); + + myMatrix temp2CovariancePositionEstimationKplus1Kplus13x3(3,3); + myMatrix tempCovariancePositionEstimationKplus1Kplus13x3(3,3); + myMatrix tempCovariancePositionEstimationKplus1Kplus13x2(3,2); + myMatrix tempCovariancePositionEstimationKplus1Kplus13x1(3,1); + //we do not use the composite measurement jacobian (16), we directly use the values from the measurement jacobian (jacobianOfObservation) - switch(nbPassed){ case 0://none //nothings happens it's okay break; case 1://left - compositeInnovationCovariance1x1=R_right + jacobianOfObservationRight[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]); - - kalmanGain3X1[0][0]=(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][2])/compositeInnovationCovariance1x1; - kalmanGain3X1[1][0]=(covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][2])/compositeInnovationCovariance1x1; - kalmanGain3X1[2][0]=(covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2])/compositeInnovationCovariance1x1; - - xEstimatedKNext+= kalmanGain3X1[0][0]*innovationRight; - yEstimatedKNext+= kalmanGain3X1[1][0]*innovationRight; - thetaWorldEstimatedKNext+= kalmanGain3X1[2][0]*innovationRight; + //compute compositeMeasurementJacobian + //here compositeMeasurementJacobian= jacobianOfObservationLeft + compositeMeasurementJacobian1x3.fillByCopy(jacobianOfObservationLeft); + //get the compositeMeasurementJacobianTranspose + compositeMeasurementJacobian1x3Transpose.fillWithTranspose(compositeMeasurementJacobian1x3); + //compute compositeInnovation + //here compositeInnovation=innovationLeft + compositeInnovation1x1.data[0][0]=innovationLeft; + //compute compositeInnovationCovariance=compositeMeasurementJacobian*covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranpose + //add the right R on the diagonal + //here compositeInnovationCovariance=innovationCovarianceLeft + compositeInnovationCovariance1x1.data[0][0]=innovationCovarianceLeft; + //get the inverse of the compositeInnovationCovariance + compositeInnovationCovariance1x1Inverse.data[0][0]=1/innovationCovarianceLeft; + //compute KalmanGain=covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranspose*Inverse(compositeInnovationCovariance) + tempKalmanGain3X1.fillByMultiplication(covariancePositionEstimationKplus1K,compositeMeasurementJacobian1x3Transpose); + kalmanGain3X1.fillByMultiplication(tempKalmanGain3X1,compositeInnovationCovariance1x1Inverse); + //get the transpose of the kalman gain + kalmanGain3X1Transpose.fillWithTranspose(kalmanGain3X1); + //update pose estimation=old pose estimation + kalman gain*compositeInnovation + tempPoseKplus1Kplus1.fillByMultiplication(kalmanGain3X1,compositeInnovation1x1); + poseKplus1Kplus1.addition(tempPoseKplus1Kplus1); + //compute covariancePositionEstimationKplus1Kplus1=covariancePositionEstimationKplus1K-kalmanGain*compositeInnovationCovariance*KalmanGainTranspose + tempCovariancePositionEstimationKplus1Kplus13x1.fillByMultiplication(kalmanGain3X1,compositeInnovationCovariance1x1); + tempCovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(tempCovariancePositionEstimationKplus1Kplus13x1,kalmanGain3X1Transpose); + covariancePositionEstimationKplus1Kplus1.subtraction(tempCovariancePositionEstimationKplus1Kplus13x3); - covariancePositionEstimationK[0][0]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[0][0],2) + covariancePositionEstimationK[0][0]; - covariancePositionEstimationK[0][1]=covariancePositionEstimationK[0][1] - kalmanGain3X1[0][0]*kalmanGain3X1[1][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[0][2]=covariancePositionEstimationK[0][2] - kalmanGain3X1[0][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[1][0]=covariancePositionEstimationK[1][0] - kalmanGain3X1[0][0]*kalmanGain3X1[1][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[1][1]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[1][0],2) + covariancePositionEstimationK[1][1]; - covariancePositionEstimationK[1][2]=covariancePositionEstimationK[1][2] - kalmanGain3X1[1][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[2][0]=covariancePositionEstimationK[2][0] - kalmanGain3X1[0][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[2][1]=covariancePositionEstimationK[2][1] - kalmanGain3X1[1][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[2][2]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[2][0],2) + covariancePositionEstimationK[2][2]; - break; case 10://front - - compositeInnovationCovariance1x1=R_front + jacobianOfObservationFront[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]); + //compute compositeMeasurementJacobian + compositeMeasurementJacobian1x3.fillByCopy(jacobianOfObservationFront); + //get the compositeMeasurementJacobianTranspose + compositeMeasurementJacobian1x3Transpose.fillWithTranspose(compositeMeasurementJacobian1x3); + //compute compositeInnovation + compositeInnovation1x1.data[0][0]=innovationFront; + //compute compositeInnovationCovariance=compositeMeasurementJacobian*covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranpose + //add the right R on the diagonal + compositeInnovationCovariance1x1.data[0][0]=innovationCovarianceFront; + //get the inverse of the compositeInnovationCovariance + compositeInnovationCovariance1x1Inverse.data[0][0]=1/innovationCovarianceFront; + //compute KalmanGain=covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranspose*Inverse(compositeInnovationCovariance) + tempKalmanGain3X1.fillByMultiplication(covariancePositionEstimationKplus1K,compositeMeasurementJacobian1x3Transpose); + kalmanGain3X1.fillByMultiplication(tempKalmanGain3X1,compositeInnovationCovariance1x1Inverse); + //get the transpose of the kalman gain + kalmanGain3X1Transpose.fillWithTranspose(kalmanGain3X1); + //update pose estimation=old pose estimation + kalman gain*compositeInnovation + tempPoseKplus1Kplus1.fillByMultiplication(kalmanGain3X1,compositeInnovation1x1); + poseKplus1Kplus1.addition(tempPoseKplus1Kplus1); + //compute covariancePositionEstimationKplus1Kplus1=covariancePositionEstimationKplus1K-kalmanGain*compositeInnovationCovariance*KalmanGainTranspose + tempCovariancePositionEstimationKplus1Kplus13x1.fillByMultiplication(kalmanGain3X1,compositeInnovationCovariance1x1); + tempCovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(tempCovariancePositionEstimationKplus1Kplus13x1,kalmanGain3X1Transpose); + covariancePositionEstimationKplus1Kplus1.subtraction(tempCovariancePositionEstimationKplus1Kplus13x3); - kalmanGain3X1[0][0]=(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][2])/compositeInnovationCovariance1x1; - kalmanGain3X1[1][0]=(covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][2])/compositeInnovationCovariance1x1; - kalmanGain3X1[2][0]=(covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2])/compositeInnovationCovariance1x1; - - xEstimatedKNext+= kalmanGain3X1[0][0]*innovationFront; - yEstimatedKNext+= kalmanGain3X1[1][0]*innovationFront; - thetaWorldEstimatedKNext+= kalmanGain3X1[2][0]*innovationFront; - - covariancePositionEstimationK[0][0]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[0][0],2) + covariancePositionEstimationK[0][0]; - covariancePositionEstimationK[0][1]=covariancePositionEstimationK[0][1] - kalmanGain3X1[0][0]*kalmanGain3X1[1][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[0][2]=covariancePositionEstimationK[0][2] - kalmanGain3X1[0][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[1][0]=covariancePositionEstimationK[1][0] - kalmanGain3X1[0][0]*kalmanGain3X1[1][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[1][1]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[1][0],2) + covariancePositionEstimationK[1][1]; - covariancePositionEstimationK[1][2]=covariancePositionEstimationK[1][2] - kalmanGain3X1[1][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[2][0]=covariancePositionEstimationK[2][0] - kalmanGain3X1[0][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[2][1]=covariancePositionEstimationK[2][1] - kalmanGain3X1[1][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[2][2]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[2][0],2) + covariancePositionEstimationK[2][2]; - break; case 11://left and front - compositeInnovationCovariance2x2[0][0]=R_front + jacobianOfObservationFront[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]); - compositeInnovationCovariance2x2[0][1]=jacobianOfObservationLeft[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][2]) + jacobianOfObservationLeft[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][2]) + jacobianOfObservationLeft[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]); - compositeInnovationCovariance2x2[1][0]=jacobianOfObservationFront[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationFront[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationFront[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]); - compositeInnovationCovariance2x2[1][1]=R_left + jacobianOfObservationLeft[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]); - - kalmanGain3X2[0][0]=(compositeInnovationCovariance2x2[1][1]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[1][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - kalmanGain3X2[0][1]=(compositeInnovationCovariance2x2[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[0][1]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - kalmanGain3X2[1][0]=(compositeInnovationCovariance2x2[1][1]*(covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[1][0]*(covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - kalmanGain3X2[1][1]=(compositeInnovationCovariance2x2[0][0]*(covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[0][1]*(covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - kalmanGain3X2[2][0]=(compositeInnovationCovariance2x2[1][1]*(covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[1][0]*(covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - kalmanGain3X2[2][1]=(compositeInnovationCovariance2x2[0][0]*(covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[0][1]*(covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - - xEstimatedKNext+= kalmanGain3X2[0][0]*innovationFront + kalmanGain3X2[0][1]*innovationLeft; - yEstimatedKNext+= kalmanGain3X2[1][0]*innovationFront + kalmanGain3X2[1][1]*innovationLeft; - thetaWorldEstimatedKNext+= kalmanGain3X2[2][0]*innovationFront + kalmanGain3X2[2][1]*innovationLeft; + //compute compositeMeasurementJacobian + compositeMeasurementJacobian2x3.data[0][0]=jacobianOfObservationLeft.data[0][0]; + compositeMeasurementJacobian2x3.data[0][1]=jacobianOfObservationLeft.data[0][1]; + compositeMeasurementJacobian2x3.data[0][2]=jacobianOfObservationLeft.data[0][2]; + + compositeMeasurementJacobian2x3.data[1][0]=jacobianOfObservationFront.data[0][0]; + compositeMeasurementJacobian2x3.data[1][1]=jacobianOfObservationFront.data[0][1]; + compositeMeasurementJacobian2x3.data[1][2]=jacobianOfObservationFront.data[0][2]; + + //get the compositeMeasurementJacobianTranspose + compositeMeasurementJacobian2x3Transpose.fillWithTranspose(compositeMeasurementJacobian2x3); + //compute compositeInnovation + compositeInnovation2x1.data[0][0]=innovationLeft; + compositeInnovation2x1.data[1][0]=innovationFront; + //compute compositeInnovationCovariance=compositeMeasurementJacobian*covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranpose+R + tempCompositeInnovationCovariance2x2.fillByMultiplication(compositeMeasurementJacobian2x3,covariancePositionEstimationKplus1K); + compositeInnovationCovariance2x2.fillByMultiplication(tempCompositeInnovationCovariance2x2,compositeMeasurementJacobian2x3Transpose); + //add the right R on the diagonal + compositeInnovationCovariance2x2.data[0][0]+=R_left; + compositeInnovationCovariance2x2.data[1][1]+=R_front; + //get the inverse of the compositeInnovationCovariance + compositeInnovationCovariance2x2Inverse.fillWithInverse(compositeInnovationCovariance2x2); + //compute KalmanGain=covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranspose*Inverse(compositeInnovationCovariance) + tempKalmanGain3X2.fillByMultiplication(covariancePositionEstimationKplus1K,compositeMeasurementJacobian2x3Transpose); + kalmanGain3X2.fillByMultiplication(tempKalmanGain3X2,compositeInnovationCovariance2x2Inverse); + //get the transpose of the kalman gain + kalmanGain3X2Transpose.fillWithTranspose(kalmanGain3X2); + //update pose estimation=old pose estimation + kalman gain*compositeInnovation + tempPoseKplus1Kplus1.fillByMultiplication(kalmanGain3X2,compositeInnovation2x1); + poseKplus1Kplus1.addition(tempPoseKplus1Kplus1); + //compute covariancePositionEstimationKplus1Kplus1=covariancePositionEstimationKplus1K-kalmanGain*compositeInnovationCovariance*KalmanGainTranspose + tempCovariancePositionEstimationKplus1Kplus13x2.fillByMultiplication(kalmanGain3X2,compositeInnovationCovariance2x2); + tempCovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(tempCovariancePositionEstimationKplus1Kplus13x2,kalmanGain3X2Transpose); + covariancePositionEstimationKplus1Kplus1.subtraction(tempCovariancePositionEstimationKplus1Kplus13x3); + + break; - covariancePositionEstimationK[0][0]=covariancePositionEstimationK[0][0] - kalmanGain3X2[0][0]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[0][1]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[0][1]=covariancePositionEstimationK[0][1] - kalmanGain3X2[1][0]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[1][1]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[0][2]=covariancePositionEstimationK[0][2] - kalmanGain3X2[2][0]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[2][1]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[1][0]=covariancePositionEstimationK[1][0] - kalmanGain3X2[0][0]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[0][1]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[1][1]=covariancePositionEstimationK[1][1] - kalmanGain3X2[1][0]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[1][1]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[1][2]=covariancePositionEstimationK[1][2] - kalmanGain3X2[2][0]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[2][1]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[2][0]=covariancePositionEstimationK[2][0] - kalmanGain3X2[0][0]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[0][1]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[2][1]=covariancePositionEstimationK[2][1] - kalmanGain3X2[1][0]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[1][1]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[2][2]=covariancePositionEstimationK[2][2] - kalmanGain3X2[2][0]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[2][1]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][1]); - - break; case 100://right - - compositeInnovationCovariance1x1=R_right + jacobianOfObservationRight[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]); - - kalmanGain3X1[0][0]=(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][2])/compositeInnovationCovariance1x1; - kalmanGain3X1[1][0]=(covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][2])/compositeInnovationCovariance1x1; - kalmanGain3X1[2][0]=(covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2])/compositeInnovationCovariance1x1; - - xEstimatedKNext+= kalmanGain3X1[0][0]*innovationRight; - yEstimatedKNext+= kalmanGain3X1[1][0]*innovationRight; - thetaWorldEstimatedKNext+= kalmanGain3X1[2][0]*innovationRight; - - covariancePositionEstimationK[0][0]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[0][0],2) + covariancePositionEstimationK[0][0]; - covariancePositionEstimationK[0][1]=covariancePositionEstimationK[0][1] - kalmanGain3X1[0][0]*kalmanGain3X1[1][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[0][2]=covariancePositionEstimationK[0][2] - kalmanGain3X1[0][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[1][0]=covariancePositionEstimationK[1][0] - kalmanGain3X1[0][0]*kalmanGain3X1[1][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[1][1]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[1][0],2) + covariancePositionEstimationK[1][1]; - covariancePositionEstimationK[1][2]=covariancePositionEstimationK[1][2] - kalmanGain3X1[1][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[2][0]=covariancePositionEstimationK[2][0] - kalmanGain3X1[0][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[2][1]=covariancePositionEstimationK[2][1] - kalmanGain3X1[1][0]*kalmanGain3X1[2][0]*compositeInnovationCovariance1x1; - covariancePositionEstimationK[2][2]=- compositeInnovationCovariance1x1*pow(kalmanGain3X1[2][0],2) + covariancePositionEstimationK[2][2]; + //compute compositeMeasurementJacobian + compositeMeasurementJacobian1x3.fillByCopy(jacobianOfObservationRight); + //get the compositeMeasurementJacobianTranspose + compositeMeasurementJacobian1x3Transpose.fillWithTranspose(compositeMeasurementJacobian1x3); + //compute compositeInnovation + compositeInnovation1x1.data[0][0]=innovationRight; + //compute compositeInnovationCovariance=compositeMeasurementJacobian*covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranpose + //add the right R on the diagonal + compositeInnovationCovariance1x1.data[0][0]=innovationCovarianceRight; + //get the inverse of the compositeInnovationCovariance + compositeInnovationCovariance1x1Inverse.data[0][0]=1/innovationCovarianceRight; + //compute KalmanGain=covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranspose*Inverse(compositeInnovationCovariance) + tempKalmanGain3X1.fillByMultiplication(covariancePositionEstimationKplus1K,compositeMeasurementJacobian1x3Transpose); + kalmanGain3X1.fillByMultiplication(tempKalmanGain3X1,compositeInnovationCovariance1x1Inverse); + //get the transpose of the kalman gain + kalmanGain3X1Transpose.fillWithTranspose(kalmanGain3X1); + //update pose estimation=old pose estimation + kalman gain*compositeInnovation + tempPoseKplus1Kplus1.fillByMultiplication(kalmanGain3X1,compositeInnovation1x1); + poseKplus1Kplus1.addition(tempPoseKplus1Kplus1); + //compute covariancePositionEstimationKplus1Kplus1=covariancePositionEstimationKplus1K-kalmanGain*compositeInnovationCovariance*KalmanGainTranspose + tempCovariancePositionEstimationKplus1Kplus13x1.fillByMultiplication(kalmanGain3X1,compositeInnovationCovariance1x1); + tempCovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(tempCovariancePositionEstimationKplus1Kplus13x1,kalmanGain3X1Transpose); + covariancePositionEstimationKplus1Kplus1.subtraction(tempCovariancePositionEstimationKplus1Kplus13x3); break; - case 101://right and left - compositeInnovationCovariance2x2[0][0]=R_left + jacobianOfObservationLeft[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]); - compositeInnovationCovariance2x2[0][1]=jacobianOfObservationRight[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationRight[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationRight[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]); - compositeInnovationCovariance2x2[1][0]=jacobianOfObservationLeft[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][2]) + jacobianOfObservationLeft[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][2]) + jacobianOfObservationLeft[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]); - compositeInnovationCovariance2x2[1][1]=R_right + jacobianOfObservationRight[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]); - - kalmanGain3X2[0][0]=(compositeInnovationCovariance2x2[1][1]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[1][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - kalmanGain3X2[0][1]=(compositeInnovationCovariance2x2[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[0][1]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - kalmanGain3X2[1][0]=(compositeInnovationCovariance2x2[1][1]*(covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[1][0]*(covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - kalmanGain3X2[1][1]=(compositeInnovationCovariance2x2[0][0]*(covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[0][1]*(covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - kalmanGain3X2[2][0]=(compositeInnovationCovariance2x2[1][1]*(covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[1][0]*(covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - kalmanGain3X2[2][1]=(compositeInnovationCovariance2x2[0][0]*(covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[0][1]*(covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - - xEstimatedKNext+= kalmanGain3X2[0][0]*innovationLeft + kalmanGain3X2[0][1]*innovationRight; - yEstimatedKNext+= kalmanGain3X2[1][0]*innovationLeft + kalmanGain3X2[1][1]*innovationRight; - thetaWorldEstimatedKNext+= kalmanGain3X2[2][0]*innovationLeft + kalmanGain3X2[2][1]*innovationRight; - - covariancePositionEstimationK[0][0]=covariancePositionEstimationK[0][0] - kalmanGain3X2[0][0]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[0][1]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[0][1]=covariancePositionEstimationK[0][1] - kalmanGain3X2[1][0]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[1][1]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[0][2]=covariancePositionEstimationK[0][2] - kalmanGain3X2[2][0]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[2][1]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[1][0]=covariancePositionEstimationK[1][0] - kalmanGain3X2[0][0]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[0][1]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[1][1]=covariancePositionEstimationK[1][1] - kalmanGain3X2[1][0]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[1][1]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[1][2]=covariancePositionEstimationK[1][2] - kalmanGain3X2[2][0]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[2][1]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[2][0]=covariancePositionEstimationK[2][0] - kalmanGain3X2[0][0]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[0][1]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[2][1]=covariancePositionEstimationK[2][1] - kalmanGain3X2[1][0]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[1][1]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[2][2]=covariancePositionEstimationK[2][2] - kalmanGain3X2[2][0]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[2][1]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][1]); - + case 101://left and right + //compute compositeMeasurementJacobian + compositeMeasurementJacobian2x3.data[0][0]=jacobianOfObservationLeft.data[0][0]; + compositeMeasurementJacobian2x3.data[0][1]=jacobianOfObservationLeft.data[0][1]; + compositeMeasurementJacobian2x3.data[0][2]=jacobianOfObservationLeft.data[0][2]; + + compositeMeasurementJacobian2x3.data[1][0]=jacobianOfObservationRight.data[0][0]; + compositeMeasurementJacobian2x3.data[1][1]=jacobianOfObservationRight.data[0][1]; + compositeMeasurementJacobian2x3.data[1][2]=jacobianOfObservationRight.data[0][2]; + + //get the compositeMeasurementJacobianTranspose + compositeMeasurementJacobian2x3Transpose.fillWithTranspose(compositeMeasurementJacobian2x3); + //compute compositeInnovation + compositeInnovation2x1.data[0][0]=innovationLeft; + compositeInnovation2x1.data[1][0]=innovationRight; + //compute compositeInnovationCovariance=compositeMeasurementJacobian*covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranpose+R + tempCompositeInnovationCovariance2x2.fillByMultiplication(compositeMeasurementJacobian2x3,covariancePositionEstimationKplus1K); + compositeInnovationCovariance2x2.fillByMultiplication(tempCompositeInnovationCovariance2x2,compositeMeasurementJacobian2x3Transpose); + //add the right R on the diagonal + compositeInnovationCovariance2x2.data[0][0]+=R_left; + compositeInnovationCovariance2x2.data[1][1]+=R_right; + //get the inverse of the compositeInnovationCovariance + compositeInnovationCovariance2x2Inverse.fillWithInverse(compositeInnovationCovariance2x2); + //compute KalmanGain=covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranspose*Inverse(compositeInnovationCovariance) + tempKalmanGain3X2.fillByMultiplication(covariancePositionEstimationKplus1K,compositeMeasurementJacobian2x3Transpose); + kalmanGain3X2.fillByMultiplication(tempKalmanGain3X2,compositeInnovationCovariance2x2Inverse); + //get the transpose of the kalman gain + kalmanGain3X2Transpose.fillWithTranspose(kalmanGain3X2); + //update pose estimation=old pose estimation + kalman gain*compositeInnovation + tempPoseKplus1Kplus1.fillByMultiplication(kalmanGain3X2,compositeInnovation2x1); + poseKplus1Kplus1.addition(tempPoseKplus1Kplus1); + //compute covariancePositionEstimationKplus1Kplus1=covariancePositionEstimationKplus1K-kalmanGain*compositeInnovationCovariance*KalmanGainTranspose + tempCovariancePositionEstimationKplus1Kplus13x2.fillByMultiplication(kalmanGain3X2,compositeInnovationCovariance2x2); + tempCovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(tempCovariancePositionEstimationKplus1Kplus13x2,kalmanGain3X2Transpose); + covariancePositionEstimationKplus1Kplus1.subtraction(tempCovariancePositionEstimationKplus1Kplus13x3); + + break; - case 110://right and front - - compositeInnovationCovariance2x2[0][0]=R_front + jacobianOfObservationFront[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]); - compositeInnovationCovariance2x2[0][1]=jacobianOfObservationRight[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][2]) + jacobianOfObservationRight[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][2]) + jacobianOfObservationRight[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]); - compositeInnovationCovariance2x2[1][0]=jacobianOfObservationFront[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][2]) + jacobianOfObservationFront[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][2]) + jacobianOfObservationFront[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]); - compositeInnovationCovariance2x2[1][1]=R_right + jacobianOfObservationRight[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]); - - kalmanGain3X2[0][0]=(compositeInnovationCovariance2x2[1][1]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[1][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - kalmanGain3X2[0][1]=(compositeInnovationCovariance2x2[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[0][1]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - kalmanGain3X2[1][0]=(compositeInnovationCovariance2x2[1][1]*(covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[1][0]*(covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - kalmanGain3X2[1][1]=(compositeInnovationCovariance2x2[0][0]*(covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[0][1]*(covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - kalmanGain3X2[2][0]=(compositeInnovationCovariance2x2[1][1]*(covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[1][0]*(covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - kalmanGain3X2[2][1]=(compositeInnovationCovariance2x2[0][0]*(covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]) - (compositeInnovationCovariance2x2[0][1]*(covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]))/(compositeInnovationCovariance2x2[0][0]*compositeInnovationCovariance2x2[1][1] - compositeInnovationCovariance2x2[0][1]*compositeInnovationCovariance2x2[1][0]); - - xEstimatedKNext+= kalmanGain3X2[0][0]*innovationFront + kalmanGain3X2[0][1]*innovationRight; - yEstimatedKNext+= kalmanGain3X2[1][0]*innovationFront + kalmanGain3X2[1][1]*innovationRight; - thetaWorldEstimatedKNext+= kalmanGain3X2[2][0]*innovationFront + kalmanGain3X2[2][1]*innovationRight; - - covariancePositionEstimationK[0][0]=covariancePositionEstimationK[0][0] - kalmanGain3X2[0][0]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[0][1]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[0][1]=covariancePositionEstimationK[0][1] - kalmanGain3X2[1][0]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[1][1]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[0][2]=covariancePositionEstimationK[0][2] - kalmanGain3X2[2][0]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[2][1]*(kalmanGain3X2[0][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[0][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[1][0]=covariancePositionEstimationK[1][0] - kalmanGain3X2[0][0]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[0][1]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[1][1]=covariancePositionEstimationK[1][1] - kalmanGain3X2[1][0]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[1][1]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[1][2]=covariancePositionEstimationK[1][2] - kalmanGain3X2[2][0]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[2][1]*(kalmanGain3X2[1][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[1][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[2][0]=covariancePositionEstimationK[2][0] - kalmanGain3X2[0][0]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[0][1]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[2][1]=covariancePositionEstimationK[2][1] - kalmanGain3X2[1][0]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[1][1]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][1]); - covariancePositionEstimationK[2][2]=covariancePositionEstimationK[2][2] - kalmanGain3X2[2][0]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][0] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][0]) - kalmanGain3X2[2][1]*(kalmanGain3X2[2][0]*compositeInnovationCovariance2x2[0][1] + kalmanGain3X2[2][1]*compositeInnovationCovariance2x2[1][1]); - + case 110:// front and right + //compute compositeMeasurementJacobian + compositeMeasurementJacobian2x3.data[0][0]=jacobianOfObservationFront.data[0][0]; + compositeMeasurementJacobian2x3.data[0][1]=jacobianOfObservationFront.data[0][1]; + compositeMeasurementJacobian2x3.data[0][2]=jacobianOfObservationFront.data[0][2]; + + compositeMeasurementJacobian2x3.data[1][0]=jacobianOfObservationRight.data[0][0]; + compositeMeasurementJacobian2x3.data[1][1]=jacobianOfObservationRight.data[0][1]; + compositeMeasurementJacobian2x3.data[1][2]=jacobianOfObservationRight.data[0][2]; + + //get the compositeMeasurementJacobianTranspose + compositeMeasurementJacobian2x3Transpose.fillWithTranspose(compositeMeasurementJacobian2x3); + //compute compositeInnovation + compositeInnovation2x1.data[0][0]=innovationFront; + compositeInnovation2x1.data[1][0]=innovationRight; + //compute compositeInnovationCovariance=compositeMeasurementJacobian*covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranpose+R + tempCompositeInnovationCovariance2x2.fillByMultiplication(compositeMeasurementJacobian2x3,covariancePositionEstimationKplus1K); + compositeInnovationCovariance2x2.fillByMultiplication(tempCompositeInnovationCovariance2x2,compositeMeasurementJacobian2x3Transpose); + //add the right R on the diagonal + compositeInnovationCovariance2x2.data[0][0]+=R_front; + compositeInnovationCovariance2x2.data[1][1]+=R_right; + //get the inverse of the compositeInnovationCovariance + compositeInnovationCovariance2x2Inverse.fillWithInverse(compositeInnovationCovariance2x2); + //compute KalmanGain=covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranspose*Inverse(compositeInnovationCovariance) + tempKalmanGain3X2.fillByMultiplication(covariancePositionEstimationKplus1K,compositeMeasurementJacobian2x3Transpose); + kalmanGain3X2.fillByMultiplication(tempKalmanGain3X2,compositeInnovationCovariance2x2Inverse); + //get the transpose of the kalman gain + kalmanGain3X2Transpose.fillWithTranspose(kalmanGain3X2); + //update pose estimation=old pose estimation + kalman gain*compositeInnovation + tempPoseKplus1Kplus1.fillByMultiplication(kalmanGain3X2,compositeInnovation2x1); + poseKplus1Kplus1.addition(tempPoseKplus1Kplus1); + //compute covariancePositionEstimationKplus1Kplus1=covariancePositionEstimationKplus1K-kalmanGain*compositeInnovationCovariance*KalmanGainTranspose + tempCovariancePositionEstimationKplus1Kplus13x2.fillByMultiplication(kalmanGain3X2,compositeInnovationCovariance2x2); + tempCovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(tempCovariancePositionEstimationKplus1Kplus13x2,kalmanGain3X2Transpose); + covariancePositionEstimationKplus1Kplus1.subtraction(tempCovariancePositionEstimationKplus1Kplus13x3); + break; - case 111://right front and left - //get the compositeInnovationCovariance3x3 - compositeInnovationCovariance3x3[0][0]=R_front + jacobianOfObservationFront[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][2]) + jacobianOfObservationFront[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]); - compositeInnovationCovariance3x3[0][1]=jacobianOfObservationLeft[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][2]) + jacobianOfObservationLeft[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][2]) + jacobianOfObservationLeft[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]); - compositeInnovationCovariance3x3[0][2]=jacobianOfObservationRight[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][2]) + jacobianOfObservationRight[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][2]) + jacobianOfObservationRight[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]); - compositeInnovationCovariance3x3[1][0]=jacobianOfObservationFront[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationFront[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationFront[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]); - compositeInnovationCovariance3x3[1][1]=R_left + jacobianOfObservationLeft[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationLeft[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]); - compositeInnovationCovariance3x3[1][2]=jacobianOfObservationRight[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationRight[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][2]) + jacobianOfObservationRight[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]); - compositeInnovationCovariance3x3[2][0]=jacobianOfObservationFront[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][2]) + jacobianOfObservationFront[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][2]) + jacobianOfObservationFront[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]); - compositeInnovationCovariance3x3[2][1]=jacobianOfObservationLeft[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][2]) + jacobianOfObservationLeft[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][2]) + jacobianOfObservationLeft[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]); - compositeInnovationCovariance3x3[2][2]=R_right + jacobianOfObservationRight[0][0]*(covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][1]*(covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][2]) + jacobianOfObservationRight[0][2]*(covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]); - - //compute the kalman gain - kalmanGain3X3[0][0]=(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] + covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] - covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] - covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0])/(compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] + compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0]); - kalmanGain3X3[0][1]=-(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][1] + covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][0] - covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][0] - covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][0])/(compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] + compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0]); - kalmanGain3X3[0][2]=(covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2] - covariancePositionEstimationK[0][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1] + covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2] - covariancePositionEstimationK[0][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1] + covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2] - covariancePositionEstimationK[0][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1] - covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2] + covariancePositionEstimationK[0][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0] - covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2] + covariancePositionEstimationK[0][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0] - covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2] + covariancePositionEstimationK[0][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0] + covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1] - covariancePositionEstimationK[0][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0] + covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1] - covariancePositionEstimationK[0][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0] + covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1] - covariancePositionEstimationK[0][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0])/(compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] + compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0]); - kalmanGain3X3[1][0]=(covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] - covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] - covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0])/(compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] + compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0]); - kalmanGain3X3[1][1]=-(covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][1] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][0] - covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][0] - covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][0])/(compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] + compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0]); - kalmanGain3X3[1][2]=(covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2] - covariancePositionEstimationK[1][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1] + covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2] - covariancePositionEstimationK[1][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1] + covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2] - covariancePositionEstimationK[1][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1] - covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2] + covariancePositionEstimationK[1][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0] - covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2] + covariancePositionEstimationK[1][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0] - covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2] + covariancePositionEstimationK[1][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0] + covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1] - covariancePositionEstimationK[1][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0] + covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1] - covariancePositionEstimationK[1][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0] + covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1] - covariancePositionEstimationK[1][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0])/(compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] + compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0]); - kalmanGain3X3[2][0]=(covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] - covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] - covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0])/(compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] + compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0]); - kalmanGain3X3[2][1]=-(covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][2] - covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][0] - covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][0] - covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][2] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][0] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[2][1] - covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[2][0])/(compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] + compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0]); - kalmanGain3X3[2][2]=(covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2] - covariancePositionEstimationK[2][0]*jacobianOfObservationFront[0][0]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1] + covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2] - covariancePositionEstimationK[2][1]*jacobianOfObservationFront[0][1]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1] + covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2] - covariancePositionEstimationK[2][2]*jacobianOfObservationFront[0][2]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1] - covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2] + covariancePositionEstimationK[2][0]*jacobianOfObservationLeft[0][0]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0] - covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2] + covariancePositionEstimationK[2][1]*jacobianOfObservationLeft[0][1]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0] - covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2] + covariancePositionEstimationK[2][2]*jacobianOfObservationLeft[0][2]*compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0] + covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1] - covariancePositionEstimationK[2][0]*jacobianOfObservationRight[0][0]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0] + covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1] - covariancePositionEstimationK[2][1]*jacobianOfObservationRight[0][1]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0] + covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1] - covariancePositionEstimationK[2][2]*jacobianOfObservationRight[0][2]*compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0])/(compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][2] - compositeInnovationCovariance3x3[0][0]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][2] + compositeInnovationCovariance3x3[0][1]*compositeInnovationCovariance3x3[1][2]*compositeInnovationCovariance3x3[2][0] + compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][0]*compositeInnovationCovariance3x3[2][1] - compositeInnovationCovariance3x3[0][2]*compositeInnovationCovariance3x3[1][1]*compositeInnovationCovariance3x3[2][0]); - - //update the prediction - xEstimatedKNext+= kalmanGain3X3[0][0]*innovationFront + kalmanGain3X3[0][1]*innovationLeft + kalmanGain3X3[0][2]*innovationRight; - yEstimatedKNext+= kalmanGain3X3[1][0]*innovationFront + kalmanGain3X3[1][1]*innovationLeft + kalmanGain3X3[1][2]*innovationRight; - thetaWorldEstimatedKNext+= kalmanGain3X3[2][0]*innovationFront + kalmanGain3X3[2][1]*innovationLeft + kalmanGain3X3[2][2]*innovationRight; - - //update covariancePositionEstimationK - covariancePositionEstimationK[0][0]=covariancePositionEstimationK[0][0] - kalmanGain3X3[0][0]*(kalmanGain3X3[0][0]*compositeInnovationCovariance3x3[0][0] + kalmanGain3X3[0][1]*compositeInnovationCovariance3x3[1][0] + kalmanGain3X3[0][2]*compositeInnovationCovariance3x3[2][0]) - kalmanGain3X3[0][1]*(kalmanGain3X3[0][0]*compositeInnovationCovariance3x3[0][1] + kalmanGain3X3[0][1]*compositeInnovationCovariance3x3[1][1] + kalmanGain3X3[0][2]*compositeInnovationCovariance3x3[2][1]) - kalmanGain3X3[0][2]*(kalmanGain3X3[0][0]*compositeInnovationCovariance3x3[0][2] + kalmanGain3X3[0][1]*compositeInnovationCovariance3x3[1][2] + kalmanGain3X3[0][2]*compositeInnovationCovariance3x3[2][2]); - covariancePositionEstimationK[0][1]=covariancePositionEstimationK[0][1] - kalmanGain3X3[1][0]*(kalmanGain3X3[0][0]*compositeInnovationCovariance3x3[0][0] + kalmanGain3X3[0][1]*compositeInnovationCovariance3x3[1][0] + kalmanGain3X3[0][2]*compositeInnovationCovariance3x3[2][0]) - kalmanGain3X3[1][1]*(kalmanGain3X3[0][0]*compositeInnovationCovariance3x3[0][1] + kalmanGain3X3[0][1]*compositeInnovationCovariance3x3[1][1] + kalmanGain3X3[0][2]*compositeInnovationCovariance3x3[2][1]) - kalmanGain3X3[1][2]*(kalmanGain3X3[0][0]*compositeInnovationCovariance3x3[0][2] + kalmanGain3X3[0][1]*compositeInnovationCovariance3x3[1][2] + kalmanGain3X3[0][2]*compositeInnovationCovariance3x3[2][2]); - covariancePositionEstimationK[0][2]=covariancePositionEstimationK[0][2] - kalmanGain3X3[2][0]*(kalmanGain3X3[0][0]*compositeInnovationCovariance3x3[0][0] + kalmanGain3X3[0][1]*compositeInnovationCovariance3x3[1][0] + kalmanGain3X3[0][2]*compositeInnovationCovariance3x3[2][0]) - kalmanGain3X3[2][1]*(kalmanGain3X3[0][0]*compositeInnovationCovariance3x3[0][1] + kalmanGain3X3[0][1]*compositeInnovationCovariance3x3[1][1] + kalmanGain3X3[0][2]*compositeInnovationCovariance3x3[2][1]) - kalmanGain3X3[2][2]*(kalmanGain3X3[0][0]*compositeInnovationCovariance3x3[0][2] + kalmanGain3X3[0][1]*compositeInnovationCovariance3x3[1][2] + kalmanGain3X3[0][2]*compositeInnovationCovariance3x3[2][2]); - covariancePositionEstimationK[1][0]=covariancePositionEstimationK[1][0] - kalmanGain3X3[0][0]*(kalmanGain3X3[1][0]*compositeInnovationCovariance3x3[0][0] + kalmanGain3X3[1][1]*compositeInnovationCovariance3x3[1][0] + kalmanGain3X3[1][2]*compositeInnovationCovariance3x3[2][0]) - kalmanGain3X3[0][1]*(kalmanGain3X3[1][0]*compositeInnovationCovariance3x3[0][1] + kalmanGain3X3[1][1]*compositeInnovationCovariance3x3[1][1] + kalmanGain3X3[1][2]*compositeInnovationCovariance3x3[2][1]) - kalmanGain3X3[0][2]*(kalmanGain3X3[1][0]*compositeInnovationCovariance3x3[0][2] + kalmanGain3X3[1][1]*compositeInnovationCovariance3x3[1][2] + kalmanGain3X3[1][2]*compositeInnovationCovariance3x3[2][2]); - covariancePositionEstimationK[1][1]=covariancePositionEstimationK[1][1] - kalmanGain3X3[1][0]*(kalmanGain3X3[1][0]*compositeInnovationCovariance3x3[0][0] + kalmanGain3X3[1][1]*compositeInnovationCovariance3x3[1][0] + kalmanGain3X3[1][2]*compositeInnovationCovariance3x3[2][0]) - kalmanGain3X3[1][1]*(kalmanGain3X3[1][0]*compositeInnovationCovariance3x3[0][1] + kalmanGain3X3[1][1]*compositeInnovationCovariance3x3[1][1] + kalmanGain3X3[1][2]*compositeInnovationCovariance3x3[2][1]) - kalmanGain3X3[1][2]*(kalmanGain3X3[1][0]*compositeInnovationCovariance3x3[0][2] + kalmanGain3X3[1][1]*compositeInnovationCovariance3x3[1][2] + kalmanGain3X3[1][2]*compositeInnovationCovariance3x3[2][2]); - covariancePositionEstimationK[1][2]=covariancePositionEstimationK[1][2] - kalmanGain3X3[2][0]*(kalmanGain3X3[1][0]*compositeInnovationCovariance3x3[0][0] + kalmanGain3X3[1][1]*compositeInnovationCovariance3x3[1][0] + kalmanGain3X3[1][2]*compositeInnovationCovariance3x3[2][0]) - kalmanGain3X3[2][1]*(kalmanGain3X3[1][0]*compositeInnovationCovariance3x3[0][1] + kalmanGain3X3[1][1]*compositeInnovationCovariance3x3[1][1] + kalmanGain3X3[1][2]*compositeInnovationCovariance3x3[2][1]) - kalmanGain3X3[2][2]*(kalmanGain3X3[1][0]*compositeInnovationCovariance3x3[0][2] + kalmanGain3X3[1][1]*compositeInnovationCovariance3x3[1][2] + kalmanGain3X3[1][2]*compositeInnovationCovariance3x3[2][2]); - covariancePositionEstimationK[2][0]=covariancePositionEstimationK[2][0] - kalmanGain3X3[0][0]*(kalmanGain3X3[2][0]*compositeInnovationCovariance3x3[0][0] + kalmanGain3X3[2][1]*compositeInnovationCovariance3x3[1][0] + kalmanGain3X3[2][2]*compositeInnovationCovariance3x3[2][0]) - kalmanGain3X3[0][1]*(kalmanGain3X3[2][0]*compositeInnovationCovariance3x3[0][1] + kalmanGain3X3[2][1]*compositeInnovationCovariance3x3[1][1] + kalmanGain3X3[2][2]*compositeInnovationCovariance3x3[2][1]) - kalmanGain3X3[0][2]*(kalmanGain3X3[2][0]*compositeInnovationCovariance3x3[0][2] + kalmanGain3X3[2][1]*compositeInnovationCovariance3x3[1][2] + kalmanGain3X3[2][2]*compositeInnovationCovariance3x3[2][2]); - covariancePositionEstimationK[2][1]=covariancePositionEstimationK[2][1] - kalmanGain3X3[1][0]*(kalmanGain3X3[2][0]*compositeInnovationCovariance3x3[0][0] + kalmanGain3X3[2][1]*compositeInnovationCovariance3x3[1][0] + kalmanGain3X3[2][2]*compositeInnovationCovariance3x3[2][0]) - kalmanGain3X3[1][1]*(kalmanGain3X3[2][0]*compositeInnovationCovariance3x3[0][1] + kalmanGain3X3[2][1]*compositeInnovationCovariance3x3[1][1] + kalmanGain3X3[2][2]*compositeInnovationCovariance3x3[2][1]) - kalmanGain3X3[1][2]*(kalmanGain3X3[2][0]*compositeInnovationCovariance3x3[0][2] + kalmanGain3X3[2][1]*compositeInnovationCovariance3x3[1][2] + kalmanGain3X3[2][2]*compositeInnovationCovariance3x3[2][2]); - covariancePositionEstimationK[2][2]=covariancePositionEstimationK[2][2] - kalmanGain3X3[2][0]*(kalmanGain3X3[2][0]*compositeInnovationCovariance3x3[0][0] + kalmanGain3X3[2][1]*compositeInnovationCovariance3x3[1][0] + kalmanGain3X3[2][2]*compositeInnovationCovariance3x3[2][0]) - kalmanGain3X3[2][1]*(kalmanGain3X3[2][0]*compositeInnovationCovariance3x3[0][1] + kalmanGain3X3[2][1]*compositeInnovationCovariance3x3[1][1] + kalmanGain3X3[2][2]*compositeInnovationCovariance3x3[2][1]) - kalmanGain3X3[2][2]*(kalmanGain3X3[2][0]*compositeInnovationCovariance3x3[0][2] + kalmanGain3X3[2][1]*compositeInnovationCovariance3x3[1][2] + kalmanGain3X3[2][2]*compositeInnovationCovariance3x3[2][2]); - + case 111://left front and right + //compute compositeMeasurementJacobian + compositeMeasurementJacobian3x3.data[0][0]=jacobianOfObservationLeft.data[0][0]; + compositeMeasurementJacobian3x3.data[0][1]=jacobianOfObservationLeft.data[0][1]; + compositeMeasurementJacobian3x3.data[0][2]=jacobianOfObservationLeft.data[0][2]; + + compositeMeasurementJacobian3x3.data[1][0]=jacobianOfObservationFront.data[0][0]; + compositeMeasurementJacobian3x3.data[1][1]=jacobianOfObservationFront.data[0][1]; + compositeMeasurementJacobian3x3.data[1][2]=jacobianOfObservationFront.data[0][2]; + + compositeMeasurementJacobian3x3.data[2][0]=jacobianOfObservationRight.data[0][0]; + compositeMeasurementJacobian3x3.data[2][1]=jacobianOfObservationRight.data[0][1]; + compositeMeasurementJacobian3x3.data[2][2]=jacobianOfObservationRight.data[0][2]; + + //get the compositeMeasurementJacobianTranspose + compositeMeasurementJacobian3x3Transpose.fillWithTranspose(compositeMeasurementJacobian3x3); + //compute compositeInnovation + compositeInnovation3x1.data[0][0]=innovationLeft; + compositeInnovation3x1.data[1][0]=innovationFront; + compositeInnovation3x1.data[2][0]=innovationRight; + //compute compositeInnovationCovariance=compositeMeasurementJacobian*covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranpose+R + tempCompositeInnovationCovariance3x3.fillByMultiplication(compositeMeasurementJacobian3x3,covariancePositionEstimationKplus1K); + compositeInnovationCovariance3x3.fillByMultiplication(tempCompositeInnovationCovariance3x3,compositeMeasurementJacobian3x3Transpose); + //add the right R on the diagonal + compositeInnovationCovariance3x3.data[0][0]+=R_left; + compositeInnovationCovariance3x3.data[1][1]+=R_front; + compositeInnovationCovariance3x3.data[2][2]+=R_right; + //get the inverse of the compositeInnovationCovariance + compositeInnovationCovariance3x3Inverse.fillWithInverse(compositeInnovationCovariance3x3); + //compute KalmanGain=covariancePositionEstimationKplus1K*compositeMeasurementJacobianTranspose*Inverse(compositeInnovationCovariance) + tempKalmanGain3X3.fillByMultiplication(covariancePositionEstimationKplus1K,compositeMeasurementJacobian3x3Transpose); + kalmanGain3X3.fillByMultiplication(tempKalmanGain3X3,compositeInnovationCovariance3x3Inverse); + //get the transpose of the kalman gain + kalmanGain3X3Transpose.fillWithTranspose(kalmanGain3X3); + //update pose estimation=old pose estimation + kalman gain*compositeInnovation + tempPoseKplus1Kplus1.fillByMultiplication(kalmanGain3X3,compositeInnovation3x1); + poseKplus1Kplus1.addition(tempPoseKplus1Kplus1); + //compute covariancePositionEstimationKplus1Kplus1=covariancePositionEstimationKplus1K-kalmanGain*compositeInnovationCovariance*KalmanGainTranspose + temp2CovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(kalmanGain3X3,compositeInnovationCovariance3x3); + tempCovariancePositionEstimationKplus1Kplus13x3.fillByMultiplication(temp2CovariancePositionEstimationKplus1Kplus13x3,kalmanGain3X3Transpose); + covariancePositionEstimationKplus1Kplus1.subtraction(tempCovariancePositionEstimationKplus1Kplus13x3); + break; } - //big question, in wich coordinate space are those measurements... - //try with world coordinate system + //update covariancePositionEstimationK =covariancePositionEstimationKplus1Kplus1 + this->covariancePositionEstimationK.fillByCopy(covariancePositionEstimationKplus1Kplus1); - this->xWorld=xEstimatedKNext; - this->yWorld=yEstimatedKNext; - this->thetaWorld=thetaWorldEstimatedKNext; + //update pose + this->xWorld=poseKplus1Kplus1.data[0][0]; + this->yWorld=poseKplus1Kplus1.data[1][0]; + this->thetaWorld=poseKplus1Kplus1.data[2][0]; - pc.printf("\r\nAfter Kalm X=%f, Y=%f, theta=%f",xEstimatedKNext,yEstimatedKNext,thetaWorldEstimatedKNext); + pc.printf("\r\nAfter Kalm X=%f, Y=%f, theta=%f",xWorld,yWorld,thetaWorld); //try with robot one /* X=xEstimatedKNext;