Increment groupID continuously
Added Kalman Filter of leg positions