File content as of revision 5:19f24c363418:

#include "MiniExplorerCoimbra.hpp"
#include "robot.h"

#define PI 3.14159

MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld, float widthRealMap, float heightRealMap):map(widthRealMap,heightRealMap,18,12),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){
    i2c1.frequency(100000);
    initRobot(); //Initializing the robot
    pc.baud(9600); // baud for the pc communication

    measure_always_on();//TODO check if needed

    this->setXYThetaAndXYThetaWorld(defaultXWorld,defaultYWorld,defaultThetaWorld);
    this->radiusWheels=3.25;
    this->distanceWheels=7.2; 
    
    this->k_linear=10;
    this->k_angular=200;
    this->khro=12;
    this->ka=30;
    this->kb=-13;
    this->kv=200;
    this->kh=200;
    this->kd=5;//previous 5
    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=600;
    this->repulsionConstantForce=0;
    
    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;
    
} 

void MiniExplorerCoimbra::setXYThetaAndXYThetaWorld(float defaultXWorld, float defaultYWorld, float defaultThetaWorld){
    this->xWorld=defaultXWorld;
    this->yWorld=defaultYWorld;
    this->thetaWorld=defaultThetaWorld;
    X=defaultYWorld;
    Y=-defaultXWorld;
	if(defaultThetaWorld < -PI/2)
		theta=PI/2+PI-defaultThetaWorld;
	else
		theta=defaultThetaWorld-PI/2;
}

void MiniExplorerCoimbra::myOdometria(){
	Odometria();
	this->xWorld=-Y;
	this->yWorld=X;
	if(theta >PI/2)
		this->thetaWorld=-PI+(theta-PI/2);
	else
		this->thetaWorld=theta+PI/2;
}

void MiniExplorerCoimbra::go_to_point(float targetXWorld, float targetYWorld) {
    
    float angleError; //angle error
    float d; //distance from target
    float k_linear=10, k_angular=200;
    float angularLeft, angularRight, linear, angular;
    int speed=300;

    do {        
        //Updating X,Y and theta with the odometry values
        this->myOdometria();

        //Computing angle error and distance towards the target value
        angleError = atan2((targetYWorld-this->yWorld),(targetXWorld-this->xWorld))-this->thetaWorld;
        if(angleError>PI) angleError=-(angleError-PI);
        else if(angleError<-PI) angleError=-(angleError+PI);
        pc.printf("\n\r error=%f",angleError);
        
        d=this->dist(this->xWorld, this->yWorld, targetXWorld, targetYWorld);
		pc.printf("\n\r dist=%f/n", d);

        //Computing linear and angular velocities
        linear=k_linear*d;
        angular=k_angular*angleError;
        angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
        angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;

        
        //Normalize speed for motors
        if(angularLeft>angularRight) {
            angularRight=speed*angularRight/angularLeft;
            angularLeft=speed;
        } else {
            angularLeft=speed*angularLeft/angularRight;
            angularRight=speed;
        }

        pc.printf("\n\r X=%f", this->xWorld);
        pc.printf("\n\r Y=%f", this->yWorld);
        pc.printf("\n\r theta=%f", this->thetaWorld);

        //Updating motor velocities
        if(angularLeft>0){
            leftMotor(1,angularLeft);
        }
        else{
            leftMotor(0,-angularLeft);
        }
        
        if(angularRight>0){
            rightMotor(1,angularRight);
        }
        else{
            rightMotor(0,-angularRight);
        }

        //wait(0.5);
    } while(d>1);

    //Stop at the end
    leftMotor(1,0);
    rightMotor(1,0);
}

void MiniExplorerCoimbra::test_procedure_lab2(int nbIteration){
	for(int i=0;i<nbIteration;i++){
        this->randomize_and_map();
        //this->print_map_with_robot_position();
    }
    while(1)
    	this->print_map_with_robot_position();
}

//generate a position randomly and makes the robot go there while updating the map
void MiniExplorerCoimbra::randomize_and_map() {
    //TODO check that it's aurelien's work
    float movementOnX=rand()%(int)(this->map.widthRealMap);
    float movementOnY=rand()%(int)(this->map.heightRealMap);

    float targetXWorld = movementOnX;
    float targetYWorld = movementOnY;
    float targetAngleWorld = 2*((float)(rand()%31416)-15708)/10000.0;
    
    //target between (0,0) and (widthRealMap,heightRealMap)
    this->go_to_point_with_angle(targetXWorld, targetYWorld, targetAngleWorld);
}

void MiniExplorerCoimbra::test_sonars_and_map(int nbIteration){
    float leftMm;
    float frontMm; 
    float rightMm;
    this->myOdometria();
    this->print_map_with_robot_position();
	for(int i=0;i<nbIteration;i++){
        leftMm = get_distance_left_sensor();
        frontMm = get_distance_front_sensor();
        rightMm = get_distance_right_sensor();
		pc.printf("\n\r 1 leftMm= %f",leftMm);
        pc.printf("\n\r 1 frontMm= %f",frontMm);
        pc.printf("\n\r 1 rightMm= %f",rightMm);
        this->update_sonar_values(leftMm, frontMm, rightMm);
        this->print_map_with_robot_position();
        wait(1);
	}
}


//generate a position randomly and makes the robot go there while updating the map
//move of targetXWorld and targetYWorld ending in a targetAngleWorld
void MiniExplorerCoimbra::go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld) {
    bool keepGoing=true;
    float leftMm;
    float frontMm; 
    float rightMm;
    float dt;
    Timer t;
    float distanceToTarget;
    do {
        //Timer stuff
        dt = t.read();
        t.reset();
        t.start();
        
        //Updating X,Y and theta with the odometry values
        this->myOdometria();
        leftMm = get_distance_left_sensor();
        frontMm = get_distance_front_sensor();
        rightMm = get_distance_right_sensor();
        //if in dangerzone 150 mm 
        if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){
            //stop motors
            leftMotor(1,0);
            rightMotor(1,0);
            //update the map
            this->update_sonar_values(leftMm, frontMm, rightMm);
            this->myOdometria();
            keepGoing=false;
            this->do_half_flip(); 
        }else{
            //if not in danger zone continue as usual
            this->update_sonar_values(leftMm, frontMm, rightMm);
            //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>2 || (abs(targetAngleWorld-this->thetaWorld)>PI/3)) && keepGoing);

    //Stop at the end
    leftMotor(1,0);
    rightMotor(1,0);
    pc.printf("\r\nReached Target!");
}

//move of targetXWorld and targetYWorld ending in a targetAngleWorld
void MiniExplorerCoimbra::go_to_point_with_angle_first_lab(float targetXWorld, float targetYWorld, float targetAngleWorld) {
    float dt;
    Timer t;
    float distanceToTarget;
    do {
        //Timer stuff
        dt = t.read();
        t.reset();
        t.start();
        
        //Updating X,Y and theta with the odometry values
        this->myOdometria();
        
        //Updating motor velocities
        distanceToTarget=this->update_angular_speed_wheels_go_to_point_with_angle(targetXWorld,targetYWorld,targetAngleWorld,dt);
    
        //wait(0.2);
        //Timer stuff
        t.stop();
        pc.printf("\n\rdist to target= %f",distanceToTarget);
        
    } while(distanceToTarget>1 || (abs(targetAngleWorld-this->thetaWorld)>0.1));

    //Stop at the end
    leftMotor(1,0);
    rightMotor(1,0);
    pc.printf("\r\nReached Target!");
}

float MiniExplorerCoimbra::update_angular_speed_wheels_go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld, float dt){
    //compute_angles_and_distance
    //atan2 take the deplacement on x and the deplacement on y as parameters
    float angleToPoint = atan2((targetYWorld-this->yWorld),(targetXWorld-this->xWorld))-this->thetaWorld;
	
	if(angleToPoint>PI) angleToPoint=-(angleToPoint-PI);
    else if(angleToPoint<-PI) angleToPoint=-(angleToPoint+PI);    
    
    //rho is the distance to the point of arrival
    float rho = dist(targetXWorld,targetYWorld,this->xWorld,this->yWorld);
    float distanceToTarget = rho;
    //TODO check that
    float beta = targetAngleWorld-angleToPoint-this->thetaWorld;        
    
    //Computing angle error and distance towards the target value
    rho += dt*(-this->khro*cos(angleToPoint)*rho);
    float temp = angleToPoint;
    angleToPoint += dt*(this->khro*sin(angleToPoint)-this->ka*angleToPoint-this->kb*beta);
    beta += dt*(-this->khro*sin(temp));

    //Computing linear and angular velocities
    float linear;
    float angular;
    if(angleToPoint>=-1.5708 && angleToPoint<=1.5708){
        linear=this->khro*rho;
        angular=this->ka*angleToPoint+this->kb*beta;
    }
    else{
        linear=-this->khro*rho;
        angular=-this->ka*angleToPoint-this->kb*beta;
    }
        
    float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
    float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
    
    //Slowing down at the end for more precision
	if (distanceToTarget<30) {
    	this->speed = distanceToTarget*10;
	}
    
    //Normalize speed for motors
    if(angularLeft>angularRight) {
        angularRight=this->speed*angularRight/angularLeft;
        angularLeft=this->speed;
    } else {
        angularLeft=this->speed*angularLeft/angularRight;
        angularRight=this->speed;
    }

    //compute_linear_angular_velocities 
    leftMotor(1,angularLeft);
    rightMotor(1,angularRight);
    
    return distanceToTarget;
}

void MiniExplorerCoimbra::update_sonar_values(float leftMm,float frontMm,float rightMm){
    float xWorldCell;
    float yWorldCell;
    float probaLeft;
    float probaFront;
    float probaRight;
    float leftCm=leftMm/10;
    float frontCm=frontMm/10;
    float rightCm=rightMm/10;
    for(int i=0;i<this->map.nbCellWidth;i++){
        for(int j=0;j<this->map.nbCellHeight;j++){
        	xWorldCell=this->map.cell_width_coordinate_to_world(i);
            yWorldCell=this->map.cell_height_coordinate_to_world(j);
            
            probaLeft=this->sonarLeft.compute_probability_t(leftCm,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld);
			probaFront=this->sonarFront.compute_probability_t(frontCm,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld);
			probaRight=this->sonarRight.compute_probability_t(rightCm,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld);
			
			/*
			pc.printf("\n\r leftCm= %f",leftCm);
	        pc.printf("\n\r frontCm= %f",frontCm);
	        pc.printf("\n\r rightCm= %f",rightCm);
	        */
	        /*
			pc.printf("\n\r probaLeft= %f",probaLeft);
	        pc.printf("\n\r probaFront= %f",probaFront);
	        pc.printf("\n\r probaRight= %f",probaRight);
	        if(probaLeft> 1 || probaLeft < 0 || probaFront> 1 || probaFront < 0 ||probaRight> 1 || probaRight < 0)){
	        	 pwm_buzzer.pulsewidth_us(250);
	            wait_ms(50);
	            pwm_buzzer.pulsewidth_us(0);
	            wait(20);
	            pwm_buzzer.pulsewidth_us(250);
	            wait_ms(50);
	            pwm_buzzer.pulsewidth_us(0);
	        } 
        	*/
        	this->map.update_cell_value(i,j,probaLeft);
        	this->map.update_cell_value(i,j,probaFront);
        	this->map.update_cell_value(i,j,probaRight);
       	}
    }
}

void MiniExplorerCoimbra::do_half_flip(){
    this->myOdometria();
    float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
    if(theta_plus_h_pi > PI)
        theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
    leftMotor(0,100);
    rightMotor(1,100);
    while(abs(theta_plus_h_pi-theta)>0.05){
        this->myOdometria();
       // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
    }
    leftMotor(1,0);
    rightMotor(1,0);    
}

//Distance computation function
float MiniExplorerCoimbra::dist(float x1, float y1, float x2, float y2){
    return sqrt(pow(y2-y1,2) + pow(x2-x1,2));
}

//use virtual force field
void MiniExplorerCoimbra::try_to_reach_target(float targetXWorld,float targetYWorld){
    //atan2 gives the angle beetween PI and -PI
    this->myOdometria();
    /*
    float deplacementOnXWorld=targetXWorld-this->xWorld;
    float deplacementOnYWorld=targetYWorld-this->yWorld;
    */
    //float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
    //pc.printf("\n angleToTarget=%f",angleToTarget);
    //turn_to_target(angleToTarget);
    //TODO IDEA check if maybe set a low max range
    //this->sonarLeft.setMaxRange(30);
    //this->sonarFront.setMaxRange(30);
    //this->sonarRight.setMaxRange(30);
    bool reached=false;
    int print=0;
    int printLimit=1000;
    while (!reached) {
        this->vff(&reached,targetXWorld,targetYWorld);
        //test_got_to_line(&reached);
        if(print==printLimit){
            leftMotor(1,0);
            rightMotor(1,0);
            this->print_map_with_robot_position_and_target(targetXWorld,targetYWorld);
            print=0;
        }else
            print+=1;
    }
    //Stop at the end
    leftMotor(1,0);
    rightMotor(1,0);
    pc.printf("\r\n target reached");
    //wait(3);//
}

void MiniExplorerCoimbra::vff(bool* reached, float targetXWorld, float targetYWorld){
    float line_a;
    float line_b;
    float line_c;
    //Updating X,Y and theta with the odometry values
    float forceXWorld=0;
    float forceYWorld=0;
    //we update the odometrie
    this->myOdometria();
    //we check the sensors
    float leftMm = get_distance_left_sensor();
    float frontMm = get_distance_front_sensor();
    float rightMm = get_distance_right_sensor();
    //update the probabilities values 
    this->update_sonar_values(leftMm, frontMm, rightMm);
    //we compute the force on X and Y
    this->compute_forceX_and_forceY(&forceXWorld, &forceYWorld,targetXWorld,targetYWorld);
    //we compute a new ine
    this->calculate_line(forceXWorld, forceYWorld, &line_a,&line_b,&line_c);
    //Updating motor velocities
    this->go_to_line(line_a,line_b,line_c,targetXWorld,targetYWorld);
    
    //wait(0.1);
    this->myOdometria();
    if(dist(this->xWorld,this->yWorld,targetXWorld,targetYWorld)<3)
        *reached=true;
}

/*angleToTarget is obtained through atan2 so it s:
< 0 if the angle is bettween PI and 2pi on a trigo circle
> 0 if it is between 0 and PI
*/
void MiniExplorerCoimbra::turn_to_target(float angleToTarget){
    this->myOdometria();
    if(angleToTarget!=0){
    	if(angleToTarget>0){   
        	leftMotor(0,1);
        	rightMotor(1,1);
	    }else{
	        leftMotor(1,1);
	        rightMotor(0,1);
	    }
    	while(abs(angleToTarget-this->thetaWorld)>0.05)
        	this->myOdometria();
    }
    leftMotor(1,0);
    rightMotor(1,0);    
}


void MiniExplorerCoimbra::print_map_with_robot_position_and_target(float targetXWorld, float targetYWorld) {
    float currProba;
    
    float heightIndiceInOrthonormal;
    float widthIndiceInOrthonormal;
    
    float widthMalus=-(3*this->map.sizeCellWidth/2);
    float widthBonus=this->map.sizeCellWidth/2;
    
    float heightMalus=-(3*this->map.sizeCellHeight/2);
    float heightBonus=this->map.sizeCellHeight/2;

    pc.printf("\n\r");
    for (int y = this->map.nbCellHeight -1; y>-1; y--) {
        for (int x= 0; x<this->map.nbCellWidth; x++) {
            heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
            widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
            if(this->yWorld >= (heightIndiceInOrthonormal+heightMalus) && this->yWorld <= (heightIndiceInOrthonormal+heightBonus) && this->xWorld >= (widthIndiceInOrthonormal+widthMalus) && this->xWorld <= (widthIndiceInOrthonormal+widthBonus))
                pc.printf(" R ");
            else{
                if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus))                    
                    pc.printf(" T ");
                else{
                    currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
                    if ( currProba < 0.5){
                        pc.printf("   ");
                        //pc.printf("%f",currProba);
                    }else{
                        if(currProba==0.5){
                            pc.printf(" . ");
                            //pc.printf("%f",currProba);
                        }else{
                            pc.printf(" X ");
                            //pc.printf("%f",currProba);
                        }
                    } 
                }
            }
        }
        pc.printf("\n\r");
    }
}

void MiniExplorerCoimbra::print_map_with_robot_position(){
    float currProba;
    
    float heightIndiceInOrthonormal;
    float widthIndiceInOrthonormal;
    
    float widthMalus=-(3*this->map.sizeCellWidth/2);
    float widthBonus=this->map.sizeCellWidth/2;
    
    float heightMalus=-(3*this->map.sizeCellHeight/2);
    float heightBonus=this->map.sizeCellHeight/2;

    pc.printf("\n\r");
    for (int y = this->map.nbCellHeight -1; y>-1; y--) {
        for (int x= 0; x<this->map.nbCellWidth; x++) {
            heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
            widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
            if(this->yWorld >= (heightIndiceInOrthonormal+heightMalus) && this->yWorld <= (heightIndiceInOrthonormal+heightBonus) && this->xWorld >= (widthIndiceInOrthonormal+widthMalus) && this->xWorld <= (widthIndiceInOrthonormal+widthBonus)){
                pc.printf(" R ");
                //pc.printf("%f",currProba);
            }else{
                currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
                if ( currProba < 0.5){
                    pc.printf("   ");
                    //pc.printf("%f",currProba);
                }else{
                    if(currProba==0.5){
                        pc.printf(" . ");
                        //pc.printf("%f",currProba);
                    }else{
                       pc.printf(" X ");
                        //pc.printf("%f",currProba);
                    }
                }
            }
        }
        pc.printf("\n\r");
    }
}

//robotX and robotY are from this->myOdometria(), calculate line_a, line_b and line_c
void MiniExplorerCoimbra::calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c){
    /*
    in the teachers maths it is 
    
    *line_a=forceY;
    *line_b=-forceX;
    
    because a*x+b*y+c=0
    a impact the vertical and b the horizontal
    and he has to put them like this because
    Robot space:      World space:
      ^                 ^
      |x                |y
   <- R                 O ->
    y                     x
    but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to 
    */
    //*line_a=forceX;
    //*line_b=forceY;
    
    *line_a=forceY;
    *line_b=-forceX;
    //because the line computed always pass by the robot center we dont need lince_c
    //*line_c=forceX*this->yWorld+forceY*this->xWorld;    
    *line_c=0;
}
//compute the force on X and Y
void MiniExplorerCoimbra::compute_forceX_and_forceY(float* forceXWorld, float* forceYWorld, float targetXWorld, float targetYWorld){
	float forceRepulsionComputedX=0;
	float forceRepulsionComputedY=0;
	     for(int i=0;i<this->map.nbCellWidth;i++){	//for each cell of the map we compute a force of repulsion
        for(int j=0;j<this->map.nbCellHeight;j++){
            this->update_force(i,j,&forceRepulsionComputedX,&forceRepulsionComputedY);
        }
    }
    //update with attraction force
    *forceXWorld=forceRepulsionComputedX;
    *forceYWorld=forceRepulsionComputedY;
    this->print_map_with_robot_position();
    pc.printf("\r\nForce X repul:%f",*forceXWorld);
    pc.printf("\r\nForce Y repul:%f",*forceYWorld);
    float distanceTargetRobot=sqrt(pow(targetXWorld-this->xWorld,2)+pow(targetYWorld-this->yWorld,2));
    if(distanceTargetRobot != 0){
        *forceXWorld+=-this->attractionConstantForce*(targetXWorld-this->xWorld)/distanceTargetRobot;
        *forceYWorld+=-this->attractionConstantForce*(targetYWorld-this->yWorld)/distanceTargetRobot;
    }else{
    	*forceXWorld+=-this->attractionConstantForce*(targetXWorld-this->xWorld)/0.01;
        *forceYWorld+=-this->attractionConstantForce*(targetYWorld-this->yWorld)/0.01;
    }
    pc.printf("\r\nForce X after attract:%f",*forceXWorld);
    pc.printf("\r\nForce Y after attract:%f",*forceYWorld);

    float amplitude=sqrt(pow(*forceXWorld,2)+pow(*forceYWorld,2));
    if(amplitude!=0){//avoid division by 0 if forceX and forceY  == 0
        *forceXWorld=*forceXWorld/amplitude;
        *forceYWorld=*forceYWorld/amplitude;
    }else{
    	*forceXWorld=*forceXWorld/0.01;
        *forceYWorld=*forceYWorld/0.01;
    }
}

//for vff
void MiniExplorerCoimbra::go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld){
    float lineAngle;
    float angleError;
    float linear;
    float angular; 
    float d;
    
    //line angle is beetween pi/2 and -pi/2

   if(line_b!=0){
        lineAngle=atan(line_a/-line_b);
    }
    else{
        lineAngle=0;
    }
    
    this->myOdometria();
	//Computing angle error
	angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
 	if(angleError>PI) 
 		angleError=-(angleError-PI);
    else 
    	if(angleError<-PI) 
    		angleError=-(angleError+PI);

    //d=this->distFromLine(this->xWorld, this->yWorld, line_a, line_b, line_c);//this could be 0
	d=0;
    //Calculating velocities
    linear= this->kv*(3.14);
    angular=-this->kd*d + this->kh*angleError;

    float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
	float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
    
   //Normalize speed for motors
    if(abs(angularLeft)>abs(angularRight)) {  
        angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
        angularLeft=this->speed*this->sign1(angularLeft);
    }
    else {
        angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
        angularRight=this->speed*this->sign1(angularRight);
    }
    
    pc.printf("\r\nd = %f", d);
    pc.printf("\r\nerror = %f, lineAngle=%f, robotAngle=%f\n", angleError,lineAngle,this->thetaWorld);
    
    leftMotor(this->sign2(angularLeft),abs(angularLeft));
    rightMotor(this->sign2(angularRight),abs(angularRight));
}

void MiniExplorerCoimbra::go_to_line_first_lab(float line_a, float line_b, float line_c){
    float lineAngle;
    float angleError;
    float linear;
    float angular;
    float d;
    
    //line angle is beetween pi/2 and -pi/2
    	
    if(line_b!=0){
        lineAngle=atan(line_a/-line_b);
    }
    else{
        lineAngle=1.5708;
    }
    
    do{
    	this->myOdometria();
	    //Computing angle error
	    pc.printf("\r\nline angle = %f", lineAngle);
	    pc.printf("\r\nthetaWorld = %f", thetaWorld);
	    angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
	    
	    if(angleError>PI) 
	    	angleError=-(angleError-PI);
	    else 
	    	if(angleError<-PI) 
	    		angleError=-(angleError+PI);
	    
	    pc.printf("\r\nangleError = %f\n", angleError);
	    d=this->distFromLine(xWorld, yWorld, line_a, line_b, line_c);
		pc.printf("\r\ndistance to line = %f", d);
		
	    //Calculating velocities
	    linear= this->kv*(3.14);
	    angular=-this->kd*d + this->kh*angleError;
	
	    float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
	    float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
	    
	    //Normalize speed for motors
	    if(abs(angularLeft)>abs(angularRight)) {  
	        angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
	        angularLeft=this->speed*this->sign1(angularLeft);
	    }
	    else {
	        angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
	        angularRight=this->speed*this->sign1(angularRight);
	    }
	    
	    leftMotor(this->sign2(angularLeft),abs(angularLeft));
	    rightMotor(this->sign2(angularRight),abs(angularRight));
    }while(1);
}

void MiniExplorerCoimbra::update_force(int widthIndice, int heightIndice, float* forceRepulsionComputedX, float* forceRepulsionComputedY ){
    //get the coordonate of the map and the robot in the ortonormal space
    float xWorldCell=this->map.cell_width_coordinate_to_world(widthIndice);
    float yWorldCell=this->map.cell_height_coordinate_to_world(heightIndice);
    //compute the distance beetween the cell and the robot
    float distanceCellToRobot=sqrt(pow(xWorldCell-this->xWorld,2)+pow(yWorldCell-this->yWorld,2));
    float probaCell;
    //check if the cell is in range
    float anglePointToRobot=atan2(yWorldCell-this->yWorld,xWorldCell-this->xWorld);//like world system
     float temp1;
     float temp2;       
    if(distanceCellToRobot <= this->rangeForce) {
        probaCell=this->map.get_proba_cell(widthIndice,heightIndice);
        pc.printf("\r\nupdate force proba:%f",probaCell);
        temp1=this->repulsionConstantForce*probaCell/pow(distanceCellToRobot,2);
        temp2=(xWorldCell-this->xWorld)/distanceCellToRobot;
        *forceRepulsionComputedX+=temp1*temp2;
        temp2=(yWorldCell-this->yWorld)/distanceCellToRobot;
        *forceRepulsionComputedY+=temp1*temp2;
    }
}

//return 1 if positiv, -1 if negativ
float MiniExplorerCoimbra::sign1(float value){
    if(value>=0) 
        return 1;
    else 
        return -1;
}

//return 1 if positiv, 0 if negativ
int MiniExplorerCoimbra::sign2(float value){
    if(value>=0) 
        return 1;
    else 
        return 0;
}

float MiniExplorerCoimbra::distFromLine(float robot_x, float robot_y, float line_a, float line_b, float line_c){
    return abs((line_a*robot_x+line_b*robot_y+line_c)/sqrt(line_a*line_a+line_b*line_b));
}

//4th LAB
//starting position lower left

void MiniExplorerCoimbra::test_procedure_lab_4(float sizeX, float sizeY, int nbRectangle){
	
	this->map.fill_map_with_kalman_knowledge();
	
	this->go_to_point_with_angle_kalman(this->xWorld+sizeX,this->yWorld,this->thetaWorld);
	
	/*
	for(int j=0;j<nbRectangle;j++){
		//right
		this->go_to_point_with_angle_kalman(this->xWorld+sizeX,this->yWorld,this->thetaWorld);
		this->go_turn_kalman(this->xWorld,this->yWorld,this->thetaWorld+PI/2);
		
		this->print_map_with_robot_position();
		pc.printf("\n\rX= %f",this->xWorld);
		pc.printf("\n\rY= %f",this->yWorld);
		pc.printf("\n\rtheta= %f",this->thetaWorld);
		
		//up
		this->go_to_point_with_angle_kalman(this->xWorld+sizeX,this->yWorld+sizeY,this->thetaWorld);
		this->go_turn_kalman(this->xWorld,this->yWorld,this->thetaWorld+PI/2);
		
		this->print_map_with_robot_position();
		pc.printf("\n\rX= %f",this->xWorld);
		pc.printf("\n\rY= %f",this->yWorld);
		pc.printf("\n\rtheta= %f",this->thetaWorld);
		//left
		this->go_to_point_with_angle_kalman(this->xWorld-sizeX,this->yWorld,this->thetaWorld);
		this->go_turn_kalman(this->xWorld,this->yWorld,this->thetaWorld+PI/2);
		
		this->print_map_with_robot_position();
		pc.printf("\n\rX= %f",this->xWorld);
		pc.printf("\n\rY= %f",this->yWorld);
		pc.printf("\n\rtheta= %f",this->thetaWorld);
		//down
		this->go_to_point_with_angle_kalman(this->xWorld,this->yWorld-sizeY,this->thetaWorld);
		this->go_turn_kalman(this->xWorld,this->yWorld,this->thetaWorld+PI/2);
		
		this->print_map_with_robot_position();
		pc.printf("\n\rX= %f",this->xWorld);
		pc.printf("\n\rY= %f",this->yWorld);
		pc.printf("\n\rtheta= %f",this->thetaWorld);
		
	}
	*/
}

//move of targetXWorld and targetYWorld ending in a targetAngleWorld
void MiniExplorerCoimbra::go_turn_kalman(float targetXWorld, float targetYWorld, float targetAngleWorld) {
    //make sure the target is correct
    if(targetAngleWorld > PI)
    	targetAngleWorld=-2*PI+targetAngleWorld;
   	if(targetAngleWorld < -PI)
   		targetAngleWorld=2*PI+targetAngleWorld;
   		
	float distanceToTarget=100;
    do {
        leftMotor(1,50);
	    rightMotor(0,50);
	    this->OdometriaKalmanFilter(1,1);
	    
	    float distanceToTarget=this->dist(this->xWorld, this->yWorld, targetXWorld, targetYWorld);
        //pc.printf("\n\rdist to target= %f",distanceToTarget);
        
    } while(distanceToTarget>1 || (abs(targetAngleWorld-this->thetaWorld)>0.1));

    //Stop at the end
    leftMotor(1,0);
    rightMotor(1,0);
    pc.printf("\r\nReached Target!");
}

//move of targetXWorld and targetYWorld ending in a targetAngleWorld
void MiniExplorerCoimbra::go_straight_kalman(float targetXWorld, float targetYWorld, float targetAngleWorld) {
    //make sure the target is correct
    if(targetAngleWorld > PI)
    	targetAngleWorld=-2*PI+targetAngleWorld;
   	if(targetAngleWorld < -PI)
   		targetAngleWorld=2*PI+targetAngleWorld;
   		
	float distanceToTarget=100;;
	
    do {
        leftMotor(1,400);
	    rightMotor(1,400);
	    this->OdometriaKalmanFilter(1,1);
	    
	    float distanceToTarget=this->dist(this->xWorld, this->yWorld, targetXWorld, targetYWorld);
        pc.printf("\n\rdist to target= %f",distanceToTarget);
        
    } while(distanceToTarget>1 || (abs(targetAngleWorld-this->thetaWorld)>0.1));

    //Stop at the end
    leftMotor(1,0);
    rightMotor(1,0);
    pc.printf("\r\nReached Target!");
}

//move of targetXWorld and targetYWorld ending in a targetAngleWorld
void MiniExplorerCoimbra::go_to_point_with_angle_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(1,1);
        
        //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>1 || (abs(targetAngleWorld-this->thetaWorld)>0.1));

    //Stop at the end
    leftMotor(1,0);
    rightMotor(1,0);
    pc.printf("\r\nReached Target!");
}

void MiniExplorerCoimbra::OdometriaKalmanFilter(float encoderRightFailureRate,float encoderLeftFailureRate){
	//=====KINEMATICS===========================
	float R_cm;
	float L_cm;

	//fill R_cm and L_cm with how much is wheel has moved (custom robot.h)
	OdometriaKalman(&R_cm, &L_cm);
	
	encoderRightFailureRate=0.95;
	encoderLeftFailureRate=1;
	
	R_cm=R_cm*encoderRightFailureRate;
	L_cm=L_cm*encoderLeftFailureRate;

	float distanceMoved=(R_cm+L_cm)/2;
    float angleMoved=(R_cm-L_cm)/this->distanceWheels;

    float distanceMovedX=distanceMoved*cos(this->thetaWorld+angleMoved/2);
    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;
    
    //try with robot coordinate system
    /*
	float xEstimatedK=X;
    float yEstimatedK=Y;
    float thetaWorldEstimatedK = theta; 
    */
    //=====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)); 

    //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));
            
     //error constants...
    float kr=1;
    float kl=1;
    float covar[2][2]={{kr*abs(R_cm), 0}, {0, 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=====
    //get the estimated measure we should have according to our knowledge of the map and the previously computed localisation
    
    float leftCmEstimated=this->sonarLeft.maxRange;
    float frontCmEstimated=this->sonarFront.maxRange;
    float rightCmEstimated=this->sonarRight.maxRange;
    float xWorldCell;
    float yWorldCell;
    float currDistance;
    float xClosestCellLeft;
    float yClosestCellLeft;
    float xClosestCellFront;
    float yClosestCellFront;
    float xClosestCellRight;
    float yClosestCellRight;
    //get theorical distance to sonar
    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){
                //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);           
                if((currDistance < this->sonarLeft.maxRange) && currDistance!=-1){
                    //check if distance is lower than previous, update lowest if so
                    if(currDistance < leftCmEstimated){
                        leftCmEstimated= currDistance;
                        xClosestCellLeft=xWorldCell;
                        yClosestCellLeft=yWorldCell;
                    }
            	}
                //check front
                currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,xEstimatedK,yEstimatedK,thetaWorldEstimatedK);           
                if((currDistance < this->sonarFront.maxRange) && currDistance!=-1){
                    //check if distance is lower than previous, update lowest if so
                    if(currDistance < frontCmEstimated){
                        frontCmEstimated= currDistance;
                        xClosestCellFront=xWorldCell;
                        yClosestCellFront=yWorldCell;
                    }
                }
                //check right
                currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,xEstimatedK,yEstimatedK,thetaWorldEstimatedK);              
                if((currDistance < this->sonarRight.maxRange) && currDistance!=-1){
                    //check if distance is lower than previous, update lowest if so
                    if(currDistance < rightCmEstimated){
                        rightCmEstimated= currDistance;
                        xClosestCellRight=xWorldCell;
                        yClosestCellRight=yWorldCell;
                    }
                }
            }
        }
    }

    //check measurements from sonars, see if they passed the validation gate
    float leftCm = get_distance_left_sensor()/10;
    float frontCm = get_distance_front_sensor()/10;
    float rightCm = get_distance_right_sensor()/10;
    //if superior to sonar range, put the value to sonar max range + 1
    if(leftCm > this->sonarLeft.maxRange)
        leftCm=this->sonarLeft.maxRange;
    if(frontCm > this->sonarFront.maxRange)
        frontCm=this->sonarFront.maxRange;
    if(rightCm > this->sonarRight.maxRange)
        rightCm=this->sonarRight.maxRange;

    //======INNOVATION========
    //get the innoncation: the value of the difference between the actual measure and what we anticipated
    float innovationLeft=leftCm-leftCmEstimated;
    float innovationFront=frontCm-frontCmEstimated;
    float innovationRight=-rightCm-rightCmEstimated;
    //compute jacobian of observation
    float jacobianOfObservationLeft[1][3];
    float jacobianOfObservationRight[1][3];
    float jacobianOfObservationFront[1][3];
    float xSonarLeft=xEstimatedK+this->sonarLeft.distanceX;
    float ySonarLeft=yEstimatedK+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));
    //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));
    //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));

    //error constants 0,0,0 sonars perfect;  must be found by experimenting; gives mean and standanrt deviation
    //let's assume
    //in centimeters
    float R_front=4;
    float R_left=4;
    float R_right=4;
    
    //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]);

    //check if it pass the validation gate 
    float gateScoreLeft=innovationLeft*innovationLeft/innovationCovarianceLeft;
    float gateScoreFront=innovationFront*innovationFront/innovationCovarianceFront;
    float gateScoreRight=innovationRight*innovationRight/innovationCovarianceRight;
    int leftPassed=0;
    int frontPassed=0;
    int rightPassed=0;

	//5cm -> 25
    int errorsquare=25;//constant error

    if(gateScoreLeft <= errorsquare)
        leftPassed=1;
    if(gateScoreFront <= errorsquare)
        frontPassed=10;
    if(gateScoreRight <= errorsquare)
        rightPassed=100;
    //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}};

    //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;
            
            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]);
        
            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;
            
            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];

            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]);
            
            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]);

            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]);
            			
            break;
	}
		//big question, in wich coordinate space are those measurements...
		//try with world coordinate system
		
		this->xWorld=xEstimatedKNext;
		this->yWorld=yEstimatedKNext;
		this->thetaWorld=thetaWorldEstimatedKNext;
		
		
		//try with robot one
		/*
		X=xEstimatedKNext;
		Y=yEstimatedKNext;
		theta=thetaWorldEstimatedKNext;
		this->xWorld=-Y;
		this->yWorld=X;
		if(theta >PI/2)
			this->thetaWorld=-PI+(theta-PI/2);
		else
			this->thetaWorld=theta+PI/2;
		
		
		this->print_map_with_robot_position();
		pc.printf("\n\rX= %f",this->xWorld);
		pc.printf("\n\rY= %f",this->yWorld);
		pc.printf("\n\rtheta= %f",this->thetaWorld);
		*/
    	//update odometrie X Y theta...
}