File content as of revision 38:5ed7c79fb724:

    
/*
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?)
*/



    
    
    
//for those who passed
//compute composite innovation
//compute composite measurement Jacobian
//compute kalman gain
//compute updated robot position estimate
//store the resultant covariance for next estimation

//return resultant covariance

//compute predicted localisation
float xEstimatedK=this->xWorld;
float yEstimatedK=this->yWorld;
float thetaWorldEstimatedK=this->thetaWorld;
float distanceMoved;
float angleMoved;

float xEstimatedKNext=xEstimatedK+distanceMoved*cos(angleMoved);
float yEstimatedKNext=yEstimatedK+distanceMoved*sin(angleMoved);
float thetaEstimatedKNext=this->thetaWorld+angleMoved;
//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+1;
if(frontCm > this->sonarFront.maxRange)
    frontCm=this->sonarFront.maxRange+1;
if(rightCm > this->sonarRight.maxRange)
    rightCm=this->sonarRight.maxRange+1;
//get the estimated measure we should have according to our knowledge of the map and the previously computed localisation
float leftCmEstimated=this->sonarLeft.maxRange+1;
float frontCmEstimated=this->sonarFront.maxRange+1;
float rightCmEstimated=this->sonarRight.maxRange+1;
float xWorldCell;
float yWorldCell;
float currDistance;
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;
            }
            //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;
            }
            //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;
            }
        }
    }
}
//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];
jacobianOfObservationLeft[0]=()/leftCmEstimated
jacobianOfObservationLeft[1]=
jacobianOfObservationLeft[2]=
//check if it pass the validation gate 
bool leftPassed=false;
bool frontPassed=false;
bool rightPassed=false;