Introduced constants and organized code for kalman filtering.