Improve visibility (the true pose, axis)
Make code more neat
Add more experiments using measurement noise
Change the naming convention