config.mat:
    
    - camera_to_body: rigid-body transform from camera to body

        - R: [2x2 double], the rotation matrix
        - T: [2x1 double], the translation (in centimeters)

    - landmark: the (x,y) coordinates of the corners of each image in the world frame (in centimeters)
   
        - xpos: [Nx4 double], the x coordinates of the corners (starting from the upperleft one and going clockwise)
        - ypos: [Nx4 double], the y coordinates of the corners

    - pose_init: the initial pose and standard deviation for the pose, (x,y) are in centimeters, and theta is in degrees
     
        - x0, y0, theta0: [double], the initial pose
        - sigma_x, sigma_y, sigma_theta: [double], the initial standard deviation for the pose (not the variance!)
 
