Situational Awareness and Collision Mitigation for Heavy Equipment