diff --git a/crosbot_graphslam/src/graphSlam.cpp b/crosbot_graphslam/src/graphSlam.cpp
index 77ae88ac2563ae453f7c7cf70d75e5a5328f00ab..80db7c3fd430aee048aaee616171218bc1006cf1 100644
--- a/crosbot_graphslam/src/graphSlam.cpp
+++ b/crosbot_graphslam/src/graphSlam.cpp
@@ -31,10 +31,7 @@ void GraphSlam::initialise(ros::NodeHandle &nh) {
    paramNH.param<int>("InformationScaleFactor", InformationScaleFactor, 1000000);
    paramNH.param<double>("MaxCovar", MaxCovar, 1.5);
    paramNH.param<double>("CorrelationThreshold", CorrelationThreshold, 3.4);
-   //paramNH.param<double>("CorrelationThreshold", CorrelationThreshold, 3.6);
    paramNH.param<int>("MinGoodCount", MinGoodCount, 10);
-   //paramNH.param<int>("FinalMinGoodCount", FinalMinGoodCount, 175);
-   //paramNH.param<int>("FinalMinGoodCount", FinalMinGoodCount, 225);
    paramNH.param<int>("FinalMinGoodCount", FinalMinGoodCount, 200);
    paramNH.param<int>("MaxIterations", MaxIterations, 50);
    paramNH.param<double>("MaxErrorTheta", MaxErrorTheta, 0.03);
diff --git a/crosbot_graphslam/src/graphSlamCPU.cpp b/crosbot_graphslam/src/graphSlamCPU.cpp
index 9f0fe3258c89e03d11dc3743f1124e0f3a5c7b0a..520d43bf28a5fbd993ec5369e0ba1e30e967e25b 100644
--- a/crosbot_graphslam/src/graphSlamCPU.cpp
+++ b/crosbot_graphslam/src/graphSlamCPU.cpp
@@ -178,6 +178,7 @@ void GraphSlamCPU::updateTrack(Pose icpPose, PointCloudPtr cloud, ros::Time stam
    oldICPPose.getYPR(yo, po, ro);
    diffTh = yi - yo;
    ANGNORM(diffTh);
+
    //Update the slam position
    double ys, ps, rs;
    slamPose.getYPR(ys, ps, rs);
@@ -189,11 +190,13 @@ void GraphSlamCPU::updateTrack(Pose icpPose, PointCloudPtr cloud, ros::Time stam
    slamPose.position.x += cosTh * diffX - sinTh * diffY;
    slamPose.position.y += sinTh * diffX + cosTh * diffY;
    slamPose.setYPR(ys, pi, ri);
+
    //Update the offset in the current local map
    offsetFromParentX += diffX;
    offsetFromParentY += diffY;
    offsetFromParentTh += diffTh;
    ANGNORM(offsetFromParentTh);
+
    //Need to convert diffX and diffY to be relative to the current local map
    cosTh = cos(-common->startICPTh);
    sinTh = sin(-common->startICPTh);
@@ -2699,7 +2702,7 @@ void GraphSlamCPU::optimiseGraph(int type) {
       int constraintIndex = common->constraintIndex[lastFullLoopIndex + 1];
       //startingNode = common->loopConstraintJ[constraintIndex];
       if (common->constraintType[lastFullLoopIndex + 1] == 1) {
-         cout << "Error: constraint after loop cloosing constraint is another loop closing one" << endl;
+         cout << "Error: constraint after loop closing constraint is another loop closing one" << endl;
       }
       startingNode = constraintIndex - 1;