Function logic of PR2 Odometry iterativeLeastSquares

edit

The "iterativeLeastSquares" function implements an iterative least squares technique to compute odometry.

• It actually computes and returns x where A*x = b

• x: [Vx Vy W]^T of the whole robot base i.e. Vx = odom_vel_.linear.x; Vy = odom_vel_.linear.y; W = odom_vel_.angular.z

• b: [v0 0 v1 0 v2 0 v3 0]^T here v0 = wheel 0's velocity in its steering direction and the 0 = wheel 0's velocity in the direction perpendicular to its steering direction and so on.

• A: the matrix that transforms x to individual wheel velocities b.