Added test case(really small one) and printed out some results to show how data will be structured. Functionalities seem to work. More testing needed in terms of testing it with graph slam + maybe testing correctness of landmark_check as threshold value might change through the course of experiments. Also, there is one TODO in code: Check movement of robot. I mean the correlation between dx,dy and orientation of robot.
issueID #8