File content as of revision 39:890439b495e3:

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

#define PI 3.14159

MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld):map(200,200,20,20),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){
    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->khro=12;
    this->ka=30;
    this->kb=-13;
    this->kv=200;
    this->kh=200;

    this->rangeForce=30;
    this->attractionConstantForce=10000;
    this->repulsionConstantForce=1;
} 

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;
}

//generate a position randomly and makes the robot go there while updating the map
void MiniExplorerCoimbra::randomize_and_map() {
    //TODO check that it's aurelien's work
    float movementOnX=rand()%(int)(this->map.widthRealMap/2);
    float movementOnY=rand()%(int)(this->map.heightRealMap/2);
    
    float signOfX=rand()%2;
    if(signOfX < 1)
        signOfX=-1;
    float signOfY=rand()%2;
    if(signOfY  < 1)
        signOfY=-1;
        
    float targetXWorld = movementOnX*signOfX;
    float targetYWorld = movementOnY*signOfY;
    float targetAngleWorld = 2*((float)(rand()%31416)-15708)/10000.0;
    this->go_to_point_with_angle(targetXWorld, targetYWorld, targetAngleWorld);
}

//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 
        if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){
            leftMotor(1,0);
            rightMotor(1,0);
            this->update_sonar_values(leftMm, frontMm, rightMm);
            //TODO Giorgos maybe you can also test the do_half_flip() function
            this->myOdometria();
            //do a flip TODO
            keepGoing=false;
            this->do_half_flip();   
        }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\r dist to target= %f",distanceToTarget);
        }
    } while(distanceToTarget>1 && (abs(targetAngleWorld-this->thetaWorld)>0.01) && keepGoing);

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

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;
    angleToPoint = atan(sin(angleToPoint)/cos(angleToPoint));
    //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 angular_left=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
    float angular_right=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
    
    //Normalize speed for motors
    if(angular_left>angular_right) {
        angular_right=this->speed*angular_right/angular_left;
        angular_left=this->speed;
    } else {
        angular_left=this->speed*angular_left/angular_right;
        angular_right=this->speed;
    }

    //compute_linear_angular_velocities 
    leftMotor(1,angular_left);
    rightMotor(1,angular_right);
    
    return distanceToTarget;
}

void MiniExplorerCoimbra::update_sonar_values(float leftMm,float frontMm,float rightMm){
    float xWorldCell;
    float yWorldCell;
    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);
        	this->map.update_cell_value(i,j,this->sonarRight.compute_probability_t(rightMm/10,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
        	this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm/10,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
        	this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm/10,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));

       	}
    }
}

//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);
    turn_to_target(angleToTarget);
    bool reached=false;
    int print=0;
    while (!reached) {
        vff(&reached,targetXWorld,targetYWorld);
        //test_got_to_line(&reached);
        if(print==10){
            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)<10)
        *reached=true;
}

//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 each cell of the map we compute a force of repulsion
     for(int i=0;i<this->map.nbCellWidth;i++){
        for(int j=0;j<this->map.nbCellHeight;j++){
            this->update_force(i,j,&forceRepulsionComputedX,&forceRepulsionComputedY);
        }
    }
    //update with attraction force
    *forceXWorld=+forceRepulsionComputedX;
    *forceYWorld=+forceRepulsionComputedY;
    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;
    }
    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;
    }
}

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));
    //check if the cell is in range
    if(distanceCellToRobot <= this->rangeForce) {
        float probaCell=this->map.get_proba_cell(widthIndice,heightIndice);
        *forceRepulsionComputedX+=this->repulsionConstantForce*probaCell*(xWorldCell-this->xWorld)/pow(distanceCellToRobot,3);
        *forceRepulsionComputedY+=this->repulsionConstantForce*probaCell*(yWorldCell-this->yWorld)/pow(distanceCellToRobot,3);
    }
}

//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;
    //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;
}

//currently line_c is not used
void MiniExplorerCoimbra::go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld){
    float lineAngle;
    float angleError;
    float linear;
    float angular; 
    
    //atan2 use the deplacement on X and the deplacement on Y
    float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
    bool aligned=false;
    
    //this condition is passed if the target is in the same direction as the robot orientation
   	if((angleToTarget>0 && this->thetaWorld > 0) || (angleToTarget<0 && this->thetaWorld <0))
    	aligned=true;
    
    //line angle is beetween pi/2 and -pi/2
    /*
    ax+by+c=0 (here c==0)
    y=x*-a/b
    if a*b > 0, the robot is going down
    if a*b <0, the robot is going up
    */	
     if(line_b!=0){
        if(!aligned)
            lineAngle=atan(-line_a/line_b);
        else
            lineAngle=atan(line_a/-line_b);
    }
    else{
        lineAngle=1.5708;
    }
    
    //Computing angle error
    angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
    angleError = atan(sin(angleError)/cos(angleError));

    //Calculating velocities
    linear=this->kv*(3.1416);
    angular=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));
}

/*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();
    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);
     if(angleToTarget>0){   
        leftMotor(0,1);
        rightMotor(1,1);
    }else{
        leftMotor(1,1);
        rightMotor(0,1);
    }
    while(abs(angleToTarget-theta_plus_h_pi)>0.05){
        this->myOdometria();
        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);
        //pc.printf("\n\r diff=%f", abs(angleToTarget-theta_plus_h_pi));
    }
    leftMotor(1,0);
    rightMotor(1,0);    
}

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

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

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

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("   ");
                    else{
                        if(currProba==0.5)
                            pc.printf(" . ");
                        else
                            pc.printf(" X ");
                    } 
                }
            }
        }
        pc.printf("\n\r");
    }
}

void MiniExplorerCoimbra::print_map() {
    float currProba;
    pc.printf("\n\r");
    for (int y = this->map.nbCellHeight -1; y>-1; y--) {
        for (int x= 0; x<this->map.nbCellWidth; x++) {
                currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
            if ( currProba < 0.5) {
                pc.printf("   ");
            } else {
                if(currProba==0.5)
                    pc.printf(" . ");
                else
                    pc.printf(" X ");
            }
        }
        pc.printf("\n\r");
    }
}

//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;
}

/*UNUSED
float MiniExplorerCoimbra::get_converted_robot_X_into_world(){
	//x world coordinate
	return this->map.nbCellWidth*this->map.sizeCellWidth-Y;
}

float MiniExplorerCoimbra::get_converted_robot_Y_into_world(){
	//y worldcoordinate
	return X;
}


//x and y passed are TargetNotMap
float get_error_angles(float x, float y,float theta){
    float angleBeetweenRobotAndTarget=atan2(y,x);
    if(y > 0){
        if(angleBeetweenRobotAndTarget < PI/2)//up right
            angleBeetweenRobotAndTarget=-PI/2+angleBeetweenRobotAndTarget;
        else//up right
            angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-PI/2;
    }else{
        if(angleBeetweenRobotAndTarget > -PI/2)//lower right
            angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-PI/2;
        else//lower left
            angleBeetweenRobotAndTarget=2*PI+angleBeetweenRobotAndTarget-PI/2;
    }
    return angleBeetweenRobotAndTarget-theta;
}
*/

   
/*
Lab4

make the robot do the same square over and over, see that at one point the errors have accumulated and it is not where the odometria think it is
solution:
(here our sensors are bad but our odometrie s okay
before each new movement we compute an estimation of where we are, compare it to what the robot think (maybe choose our estimate?)
*/

void MiniExplorerCoimbra::test_procedure_lab_4(){
    this->map.fill_map_with_initial();
    float xEstimatedK=this->xWorld;
    float yEstimatedK=this->yWorld;
    float thetaWorldEstimatedK=this->thetaWorld;
    float distanceMoved;
    float angleMoved;
    float PreviousCovarianceOdometricPositionEstimate[3][3];
    PreviousCovarianceOdometricPositionEstimate[0][0]=0;
    PreviousCovarianceOdometricPositionEstimate[0][1]=0;
    PreviousCovarianceOdometricPositionEstimate[0][2]=0;
    PreviousCovarianceOdometricPositionEstimate[1][0]=0;
    PreviousCovarianceOdometricPositionEstimate[1][1]=0;
    PreviousCovarianceOdometricPositionEstimate[1][2]=0;
    PreviousCovarianceOdometricPositionEstimate[2][0]=0;
    PreviousCovarianceOdometricPositionEstimate[2][1]=0;
    PreviousCovarianceOdometricPositionEstimate[2][2]=0;
    while(1){
        distanceMoved=50;
        angleMoved=0;
        this->go_straight_line(50);
        wait(1);
        procedure_lab_4(xEstimatedK,yEstimatedK,thetaEstimatedK,distanceMoved,angleMoved,PreviousCovarianceOdometricPositionEstimate);
        pc.printf("\n\r x:%f,y:%f,theta:%f",this->xWorld,this->yWorld,this->thetaWorld);
        pc.printf("\n\r xEstim:%f,yEstim:%f,thetaEstim:%f",xEstimatedK,yEstimatedK,thetaEstimatedK);
        this->turn_to_target(PI/2);
        distanceMoved=0;
        angleMoved=PI/2;
        procedure_lab_4(xEstimatedK,yEstimatedK,thetaEstimatedK,distanceMoved,angleMoved,PreviousCovarianceOdometricPositionEstimate);
        pc.printf("\n\r x:%f,y:%f,theta:%f",this->xWorld,this->yWorld,this->thetaWorld);
        pc.printf("\n\r xEstim:%f,yEstim:%f,thetaEstim:%f",xEstimatedK,yEstimatedK,thetaEstimatedK);
    }
}


//compute predicted localisation (assume movements are either straight lines or pure rotation), update PreviousCovarianceOdometricPositionEstimate
//TODO tweak constant rewritte it good
void MiniExplorerCoimbra::procedure_lab_4(float xEstimatedK,float yEstimatedK, float thetaWorldEstimatedK, float distanceMoved, float angleMoved, float PreviousCovarianceOdometricPositionEstimate[3][3]){

    float distanceMovedByLeftWheel=distanceMoved/2;
    float distanceMovedByRightWheel=distanceMoved/2;
    if(angleMoved !=0){
        //TODO check that not sure
        distanceMovedByLeftWheel=-angleMoved/(2*this->distanceWheels);
        distanceMovedByRightWheel=angleMoved/(2*this->distanceWheels);
    }else{
        distanceMovedByLeftWheel=distanceMoved/2;
        distanceMovedByRightWheel=distanceMoved/2;
    }
        
    float xEstimatedKNext=xEstimatedK+distanceMoved*cos(angleMoved);
    float yEstimatedKNext=yEstimatedK+distanceMoved*sin(angleMoved);
    float thetaEstimatedKNext=thetaWorldEstimatedK+angleMoved;
    
    float KRight=0.1;//error constant 
    float KLEft=0.1;//error constant 
    float motionIncrementCovarianceMatrix[2][2];
    motionIncrementCovarianceMatrix[0][0]=kRight*abs(distanceMovedRight);
    motionIncrementCovarianceMatrix[0][1]=0;
    motionIncrementCovarianceMatrix[1][0]=0;
    motionIncrementCovarianceMatrix[1][1]=kLeft*abs(distanceMovedLeft);
    
    float jacobianFp[3][3];
    jacobianFp[0][0]=1;
    jacobianFp[0][1]=0;
    jacobianFp[0][2]=-distanceMoved*sin(thetaWorldEstimatedK+angleMoved/2);
    jacobianFp[1][0]=0;
    jacobianFp[1][1]=1;
    jacobianFp[1][2]=distanceMoved*cos(thetaWorldEstimatedK+angleMoved/2);;
    jacobianFp[2][0]=0;
    jacobianFp[2][1]=0;
    jacobianFp[2][2]=1;
    
    float jacobianFpTranspose[3][3];
    jacobianFpTranspose[0][0]=1;
    jacobianFpTranspose[0][1]=0;
    jacobianFpTranspose[0][2]=0;
    jacobianFpTranspose[1][0]=0;
    jacobianFpTranspose[1][1]=1;
    jacobianFpTranspose[1][2]=0;
    jacobianFpTranspose[2][0]=-distanceMoved*sin(thetaWorldEstimatedK+angleMoved/2);
    jacobianFpTranspose[2][1]=distanceMoved*cos(thetaWorldEstimatedK+angleMoved/2);
    jacobianFpTranspose[2][2]=1;
    
    float jacobianFDeltarl[3][2];
    jacobianFDeltarl[0][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)-(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
    jacobianFDeltarl[0][1]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
    jacobianFDeltarl[1][0]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
    jacobianFDeltarl[1][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)-(distancedMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
    jacobianFDeltarl[2][0]=1/this->distanceWheels;
    jacobianFDeltarl[2][1]=-1/this->distanceWheels;
    
    float jacobianFDeltarlTranspose[2][3];//1,3,5;2,4,6
    jacobianFDeltarlTranspose[0][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)-(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
    jacobianFDeltarlTranspose[0][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
    jacobianFDeltarlTranspose[0][2]=1/this->distanceWheels;
    jacobianFDeltarlTranspose[1][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)+(distancedMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
    jacobianFDeltarlTranspose[1][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)-(distancedMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
    jacobianFDeltarlTranspose[1][2]=-1/this->distanceWheels;
    
    float tempMatrix3_3[3][3];
    for(i = 0; i < 3; ++i){//mult 3*3, 3*3
        for(j = 0; j < 3; ++j){
            for(k = 0; k < 3; ++k){
                tempMatrix3_3[i][j] += jacobianFp[i][k] * PreviousCovarianceOdometricPositionEstimate[k][j];
            }
        }
    }
    float sndTempMatrix3_3[3][3];
    for(i = 0; i < 3; ++i){//mult 3*3, 3*3
        for(j = 0; j < 3; ++j){
            for(k = 0; k < 3; ++k){
                sndTempMatrix3_3[i][j] += tempMatrix3_3[i][k] * jacobianFpTranspose[k][j];
            }
        }
    }
    float tempMatrix3_2[3][2];
    for(i = 0; i < 3; ++i){//mult 3*2 , 2,2
        for(j = 0; j < 2; ++j){
            for(k = 0; k < 2; ++k){
                tempMatrix3_2[i][j] += jacobianFDeltarl[i][k] * motionIncrementCovarianceMatrix[k][j];
            }
        }
    }
    float thrdTempMatrix3_3[3][3];
    for(i = 0; i < 3; ++i){//mult 3*2 , 2,3
        for(j = 0; j < 3; ++j){
            for(k = 0; k < 2; ++k){
                thrdTempMatrix3_3[i][j] += tempMatrix3_2[i][k] * jacobianFDeltarlTranspose[k][j];
            }
        }
    }
    float newCovarianceOdometricPositionEstimate[3][3];
    for(i = 0; i < 3; ++i){//add 3*3 , 3*3
        for(j = 0; j < 3; ++j){
            newCovarianceOdometricPositionEstimate[i][j]=sndTempMatrix3_3[i][j]+thrdTempMatrix3_3[i][j];
        }
    }
    
    //check measurements from sonars, see if they passed the validation gate
    float leftCm = get_distance_left_sensor()/10;
    float frontCm = get_distance_front_sensor()/10;
    float rightCm = get_distance_right_sensor()/10;
    //if superior to sonar range, put the value to sonar max range + 1
    if(leftCm > this->sonarLeft.maxRange)
        leftCm=this->sonarLeft.maxRange;
    if(frontCm > this->sonarFront.maxRange)
        frontCm=this->sonarFront.maxRange;
    if(rightCm > this->sonarRight.maxRange)
        rightCm=this->sonarRight.maxRange;
    //get the estimated measure we should have according to our knowledge of the map and the previously computed localisation
    float leftCmEstimated=this->sonarLeft.maxRange;
    float frontCmEstimated=this->sonarFront.maxRange;
    float rightCmEstimated=this->sonarRight.maxRange;
    float xWorldCell;
    float yWorldCell;
    float currDistance;
    float xClosetCellLeft;
    float yClosetCellLeft;
    float xClosetCellFront;
    float yClosetCellFront;
    float xClosetCellRight;
    float yClosetCellRight;
    for(int i=0;i<this->map.nbCellWidth;i++){
        for(int j=0;j<this->map.nbCellHeight;j++){
            //check if occupied, if not discard
            if(this->map.get_proba_cell(i,j)==1){
                //check if in sonar range
                xWorldCell=this->map.cell_width_coordinate_to_world(i);
                yWorldCell=this->map.cell_height_coordinate_to_world(j);
                //check left
                currDistance=this->sonarLeft.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaEstimatedKNext);           
                if((currDistance < this->sonarLeft.maxRange) && currDistance!=-1)
                    //check if distance is lower than previous, update lowest if so
                    if(currDistance < leftCmEstimated){
                        leftCmEstimated= currDistance;
                        xClosetCellLeft=xWorldCell;
                        yClosetCellLeft=yWorldCell;
                    }
                }
                //check front
                currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaEstimatedKNext);           
                if((currDistance < this->sonarFront.maxRange) && currDistance!=-1)
                    //check if distance is lower than previous, update lowest if so
                    if(currDistance < frontCmEstimated){
                        frontCmEstimated= currDistance;
                        xClosetCellFront=xWorldCell;
                        yClosetCellFront=yWorldCell;
                    }
                }
                //check right
                currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaEstimatedKNext);              
                if((currDistance < this->sonarRight.maxRange) && currDistance!=-1)
                    //check if distance is lower than previous, update lowest if so
                    if(currDistance < rightCmEstimated){
                        rightCmEstimated= currDistance;
                        xClosetCellRight=xWorldCell;
                        yClosetCellRight=yWorldCell;
                    }
                }
            }
        }
    }
    //get the innoncation: the value of the difference between the actual measure and what we anticipated
    float innovationLeft=leftCm-leftCmEstimated;
    float innovationFront=frontCm-frontCmEstimated;
    float innovationRight=-rightCm-rightCmEstimated;
    //compute jacobian of observation
    float jacobianOfObservationLeft[3];
    float jacobianOfObservationRight[3];
    float jacobianOfObservationFront[3];
    float xSonarLeft=xEstimatedKNext+this->sonarLeft.distanceX;
    float ySonarLeft=yEstimatedKNext+this->sonarLeft.distanceY;
    //left
    jacobianOfObservationLeft[0]=(xSonarLeft-xClosestCellLeft)/leftCmEstimated;
    jacobianOfObservationLeft[1]=(ySonarLeft-yClosestCellLeft)/leftCmEstimated;
    jacobianOfObservationLeft[2]=(xClosestCellLeft-xSonarLeft)*(xSonarLeft*sin(thetaEstimatedKNext)+ySonarLeft*cos(thetaEstimatedKNext)+(yClosestCellLeft-ySonarLeft)*(-xSonarLeft*cos(thetaEstimatedKNext)+ySonarLeft*sin(thetaEstimatedKNext));
    //front
    float xSonarFront=xEstimatedKNext+this->sonarFront.distanceX;
    float ySonarFront=yEstimatedKNext+this->sonarFront.distanceY;
    jacobianOfObservationFront[0]=(xSonarFront-xClosestCellFront)/FrontCmEstimated;
    jacobianOfObservationFront[1]=(ySonarFront-yClosestCellFront)/FrontCmEstimated;
    jacobianOfObservationFront[2]=(xClosestCellFront-xSonarFront)*(xSonarFront*sin(thetaEstimatedKNext)+ySonarFront*cos(thetaEstimatedKNext)+(yClosestCellFront-ySonarFront)*(-xSonarFront*cos(thetaEstimatedKNext)+ySonarFront*sin(thetaEstimatedKNext));
    //right
    float xSonarRight=xEstimatedKNext+this->sonarRight.distanceX;
    float ySonarRight=yEstimatedKNext+this->sonarRight.distanceY;
    jacobianOfObservationRight[0]=(xSonarRight-xClosestCellRight)/RightCmEstimated;
    jacobianOfObservationRight[1]=(ySonarRight-yClosestCellRight)/RightCmEstimated;
    jacobianOfObservationRight[2]=(xClosestCellRight-xSonarRight)*(xSonarRight*sin(thetaEstimatedKNext)+ySonarRight*cos(thetaEstimatedKNext)+(yClosestCellRight-ySonarRight)*(-xSonarRight*cos(thetaEstimatedKNext)+ySonarRight*sin(thetaEstimatedKNext));
    
    float innovationCovarianceLeft[3][3];
    float innovationCovarianceFront[3][3];
    float innovationCovarianceRight[3][3];
    //left
    float TempMatrix3_3InnovationLeft[3];
    TempMatrix3_3InnovationLeft[0]=jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][0];
    TempMatrix3_3InnovationLeft[1]=jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][1];
    TempMatrix3_3InnovationLeft[2]jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][2];
    float innovationConvarianceLeft=TempMatrix3_3InnovationLeft[0]*jacobianOfObservationLeft[0]+TempMatrix3_3InnovationLeft[1]*jacobianOfObservationLeft[1]+TempMatrix3_3InnovationLeft[2]*jacobianOfObservationLeft[2];
    //front
    float TempMatrix3_3InnovationFront[3];
    TempMatrix3_3InnovationFront[0]=jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][0];
    TempMatrix3_3InnovationFront[1]=jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][1];
    TempMatrix3_3InnovationFront[2]jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][2];
    float innovationConvarianceFront=TempMatrix3_3InnovationFront[0]*jacobianOfObservationFront[0]+TempMatrix3_3InnovationFront[1]*jacobianOfObservationFront[1]+TempMatrix3_3InnovationFront[2]*jacobianOfObservationFront[2];
    //right
    float TempMatrix3_3InnovationRight[3];
    TempMatrix3_3InnovationRight[0]=jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][0];
    TempMatrix3_3InnovationRight[1]=jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][1];
    TempMatrix3_3InnovationRight[2]jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][2];
    float innovationConvarianceRight=TempMatrix3_3InnovationRight[0]*jacobianOfObservationRight[0]+TempMatrix3_3InnovationRight[1]*jacobianOfObservationRight[1]+TempMatrix3_3InnovationRight[2]*jacobianOfObservationRight[2];
    
    //check if it pass the validation gate 
    float gateScoreLeft=innovationLeft*innovationLeft/innovationConvarianceLeft;
    float gateScoreFront=innovationFront*innovationFront/innovationConvarianceFront;
    float gateScoreRight=innovationRight*innovationRight/innovationConvarianceRight;
    int leftPassed=0;
    int frontPassed=0;
    int rightPassed=0;
    int e=10;//constant 
    if(gateScoreLeft < e)
        leftPassed=1;
    if(gateScoreFront < e)
        frontPassed=10;
    if(gateScoreRight < e)
        rightPassed=100;
    //for those who passed
    //compute composite innovation
    float compositeInnovation[3];
    int nbPassed=leftPassed+frontPassed+rightPassed;
    switch(nbPassed){
        case 0://none
            compositeInnovation[0]=0;
            compositeInnovation[1]=0;
            compositeInnovation[2]=0;
            break;
        case 1://left
            compositeInnovation[0]=innovationLeft;
            compositeInnovation[1]=0;
            compositeInnovation[2]=0;
            break;
        case 10://front
            compositeInnovation[0]=0;
            compositeInnovation[1]=innovationFront;
            compositeInnovation[2]=0;
            break;
        case 11://left and front
            compositeInnovation[0]=innovationLeft;
            compositeInnovation[1]=innovationFront;
            compositeInnovation[2]=0;
            break;
        case 100://right
            compositeInnovation[0]=0;
            compositeInnovation[1]=0;
            compositeInnovation[2]=innovationRight;
            break;
        case 101://right and left
            compositeInnovation[0]=innovationLeft;
            compositeInnovation[1]=0;
            compositeInnovation[2]=innovationRight;
            break;
        case 110://right and front
            compositeInnovation[0]=0;
            compositeInnovation[1]=innovationFront;
            compositeInnovation[2]=innovationRight;
            break
        case 111://right front and left
            compositeInnovation[0]=innovationLeft;
            compositeInnovation[1]=innovationFront;
            compositeInnovation[2]=innovationRight;
            break;
    }
    //compute composite measurement Jacobian
    switch(nbPassed){
        case 0://none
            break;
        case 1://left
            //compositeMeasurementJacobian = jacobianOfObservationLeft
            break;
        case 10://front
            //compositeMeasurementJacobian = jacobianOfObservationFront
            break;
        case 11://left and front
            float compositeMeasurementJacobian[6]
            compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
            compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
            compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
            compositeMeasurementJacobian[3]= jacobianOfObservationFront[0];
            compositeMeasurementJacobian[4]= jacobianOfObservationFront[1];
            compositeMeasurementJacobian[5]= jacobianOfObservationFront[2];
            break;
        case 100://right
            //compositeMeasurementJacobian = jacobianOfObservationRight
            break;
        case 101://right and left
            float compositeMeasurementJacobian[6]
            compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
            compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
            compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
            compositeMeasurementJacobian[3]= jacobianOfObservationRight[0];
            compositeMeasurementJacobian[4]= jacobianOfObservationRight[1];
            compositeMeasurementJacobian[5]= jacobianOfObservationRight[2];
    
            break;
        case 110://right and front
            float compositeMeasurementJacobian[6]
            compositeMeasurementJacobian[0]= jacobianOfObservationFront[0];
            compositeMeasurementJacobian[1]= jacobianOfObservationFront[1];
            compositeMeasurementJacobian[2]= jacobianOfObservationFront[2];
            compositeMeasurementJacobian[3]= jacobianOfObservationRight[0];
            compositeMeasurementJacobian[4]= jacobianOfObservationRight[1];
            compositeMeasurementJacobian[5]= jacobianOfObservationRight[2];
    
            break
        case 111://right front and left
            float compositeMeasurementJacobian[9]
            compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
            compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
            compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
            compositeMeasurementJacobian[3]= jacobianOfObservationFront[0];
            compositeMeasurementJacobian[4]= jacobianOfObservationFront[1];
            compositeMeasurementJacobian[5]= jacobianOfObservationFront[2];
            compositeMeasurementJacobian[6]= jacobianOfObservationRight[0];
            compositeMeasurementJacobian[7]= jacobianOfObservationRight[1];
            compositeMeasurementJacobian[8]= jacobianOfObservationRight[2];
            break;
    }
    //compute composite innovation covariance TODO dont have time, I assume you can just multiply them, can also simply all this easily
    float compositeInnovationCovariance
    switch(nbPassed){
        case 0://none
            compositeInnovationCovariance=1;
            break;
        case 1://left
            compositeInnovationCovariance = innovationConvarianceLeft;
            
            break;
        case 10://front
            compositeInnovationCovariance = innovationConvarianceFront;
            break;
        case 11://left and front
            compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceFront;
    
            break;
        case 100://right
            compositeInnovationCovariance = innovationConvarianceRight;
            break;
        case 101://right and left
            compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceRight;
    
            break;
        case 110://right and front
            compositeInnovationCovariance=innovationConvarianceFront*innovationConvarianceRight;
    
    
            break
        case 111://right front and left
            compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceFront*innovationConvarianceRight;
      
            break;
    }
    
    //compute kalman gain
    switch(nbPassed){
        //mult nbSonarPassed*3 , 3*3
        case 0://none
            break;
        case 1://left
            float kalmanGain[3][3];
            for(i = 0; i < 3; ++i){//mult 3*3, 3*1
                for(j = 0; j < 1; ++j){
                    for(k = 0; k < 3; ++k){
                        kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
                    }
                }
            }
            
            break;
        case 10://front
            float kalmanGain[3][3];
            for(i = 0; i < 3; ++i){//mult 3*3, 3*1
                for(j = 0; j < 1; ++j){
                    for(k = 0; k < 3; ++k){
                        kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
                    }
                }
            }
            break;
        case 11://left and front
            float kalmanGain[3][3];
            for(i = 0; i < 3; ++i){//mult 3*3, 3*2
                for(j = 0; j < 2; ++j){
                    for(k = 0; k < 3; ++k){
                        kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
                    }
                }
            }
    
            break;
        case 100://right
            float kalmanGain[3][3];
            for(i = 0; i < 3; ++i){//mult 3*3, 3*1
                for(j = 0; j < 1; ++j){
                    for(k = 0; k < 3; ++k){
                        kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
                    }
                }
            }
            break;
        case 101://right and left
            float kalmanGain[3][3];
            for(i = 0; i < 3; ++i){//mult 3*3, 3*2
                for(j = 0; j < 2; ++j){
                    for(k = 0; k < 3; ++k){
                        kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
                    }
                }
            }
    
            break;
        case 110://right and front
            float kalmanGain[3][3];
            for(i = 0; i < 3; ++i){//mult 3*3, 3*2
                for(j = 0; j < 2; ++j){
                    for(k = 0; k < 3; ++k){
                        kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
                    }
                }
            }
    
    
            break
        case 111://right front and left
            float kalmanGain[3][3];
            for(i = 0; i < 3; ++i){//mult 3*3, 3*3
                for(j = 0; j < 3; ++j){
                    for(k = 0; k < 3; ++k){
                        kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
                    }
                }
            }
      
            break;
    }
    for(i = 0; i < 3; ++i){//mult 3*3, 3*3
        for(j = 0; j < 3; ++j){
            kalmanGain[i][j] = kalmanGain[i][j][i][k]/compositeInnovationCovariance;
        }
    }
    //compute kalman gain * composite innovation
    //mult 3*3 , 3*1
    float result3_1[3][1];
    for(i = 0; i < 3; ++i){//mult 3*3, 3*1
        for(j = 0; j < 1; ++j){
            for(k = 0; k < 3; ++k){
                result3_1[i][j] += kalmanGain[i][k] * compositeInnovation[k];
            }
        }
    }
    //compute updated robot position estimate
    float xEstimatedKLast=xEstimatedKNext+result3_1[0];
    float yEstimatedKLast=yEstimatedKNext+result3_1[1];
    float thetaEstimatedKLast=thetaEstimatedKNext+result3_1[2];
    //store the resultant covariance for next estimation
    /*
    a b c
    d e f
    g h i
    
    a d g
    b e h
    c f i
    */
    float kalmanGainTranspose[3][3];
    kalmanGainTranspose[0][0]=kalmanGain[0][0];
    kalmanGainTranspose[0][1]=kalmanGain[1][0];
    kalmanGainTranspose[0][2]=kalmanGain[2][0];
    kalmanGainTranspose[1][0]=kalmanGain[0][1];
    kalmanGainTranspose[1][1]=kalmanGain[1][1];
    kalmanGainTranspose[1][2]=kalmanGain[2][1];
    kalmanGainTranspose[2][0]=kalmanGain[0][2];
    kalmanGainTranspose[2][1]=kalmanGain[1][2];
    kalmanGainTranspose[2][2]=kalmanGain[2][2];
    
    //mult 3*3 , 3*3
    float fithTempMatrix3_3[3][3];
    for(i = 0; i < 3; ++i){//mult 3*3 , 3*3
        for(j = 0; j < 3; ++j){
            for(k = 0; k < 3; ++k){
                fithTempMatrix3_3[i][j] += kalmanGain[i][k] * kalmanGainTranspose[k][j];
            }
        }
    }
    for(i = 0; i < 3; ++i){//add 3*3 , 3*3
        for(j = 0; j < 3; ++j){
            fithTempMatrix3_3[i][j]=fithTempMatrix3_3[i][j]*compositeInnovationCovariance;
        }
    }
    float covariancePositionEsimatedLast[3][3];
    for(i = 0; i < 3; ++i){//add 3*3 , 3*3
        for(j = 0; j < 3; ++j){
            covariancePositionEsimatedLast[i][j]=fithTempMatrix3_3[i][j]+newCovarianceOdometricPositionEstimate[i][j];
        }
    }
    //update PreviousCovarianceOdometricPositionEstimate
    for(i = 0; i < 3; ++i){
        for(j = 0; j < 3; ++j){
            PreviousCovarianceOdometricPositionEstimate[i][j]=covariancePositionEsimatedLast[i][j];
        }
    }
    
    xEstimatedK=xEstimatedKLast;
    yEstimatedK=yEstimatedKLast;
    thetaEstimatedK=thetaEstimatedKLast;
}