根据课程给定的数据, 完成基于中值法的解算

中值法修改的地方change Mapping and Map-Based Localization