Bug fix: 1. Direction of movement is calculated right.
New feature: 1. Started calculation of feature vector.
new feature: using worker height to determine it's pose ( poorly ).