Added matrix library for kalman, moved unused library