ROS 中里程计的计算代码理解
double dt = (current_time - last_time).toSec();
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
th += delta_th;
xy的坐标系代表的是将世界坐标系XOY移动到机器人中心，参考用。theta代表此时机器人相对于世界坐标系X轴旋转的角度(0-2pi),其中
double delta_x = (vx * cos(th) - vy * sin(th)) * dt; double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
可以写成 deltaX = vx*cos(theta) * dt+ vy*cost(theta + pi/2), * dt 这个是最原始的意思， 即x方向的速度在X方向（世界坐标系）的位移，理论上是这个公式，化简下，
机器人坐标系统中，一般认为，vx是机器人前进的方向，利用右手定则即可确定vy
显然，机器人的速度vy的方向角度=机器人vx角度+pi/2， 如上图
cos(theta + pi/2)=cos(theta)*cos(pi/2) - sin(theta)*sin(pi/2) = -sin(theta),
就得到delta_x = (vx * cos(th) - vy * sin(th)) * dt;
同理 delta_y = vx*sin(theta) + vy * sin(theta + pi/2) = vx*sin(theta) * dt+ vy * cos(theta) *dt
这个dt内是认为机器人走直线，保持的上一时刻的theta
