US Patent 11287819 Systems and methods for ground plane estimation

A method for estimating a ground plane includes receiving a pose of a robotic device with respect to a gravity aligned reference frame, receiving one or more locations of one or more corresponding contact points between the robotic device and a ground surface, and determining a ground plane estimation of the ground surface based on the orientation of the robotic device with respect to the gravity aligned reference frame and the one or more locations of one or more corresponding contact points between the robotic device and the ground surface. The ground plane estimation includes a ground surface contour approximation. The method further includes determining a distance between a body of the robotic device and the determined ground plane estimation and causing adjustment of the pose of the robotic device with respect to the ground surface based on the determined distance and the determined ground plane estimation.