Environmental Estimation And Smoothing Algorithms For Dynamic And Modular Robots