demo演示 运行结果 我们考虑一个机器人在平面上被少量的准时地标或_信标 包围。 机器人以轴向速度和角速度的形式接收控制动作，并且能够测量信标相对于其自身参考系的位置。 机器人位姿 X 在 SE(2) 中，信标位置 b_k 在 R^2 中，         | cos th  -sin th  x | *      X = | sin th   cos th   y | //位置和方向