Week 7. Localisation

i have two There are 3 levels of difficulty to localisation:

• Tracking: we want to know the robot’s position and keep track of it as we move
• Full Localisation: removes the assumption that we know the starting pose
• Kidnapped Robot Problem: robot knows initial pose but it has been moved

We will focus on the tracking problem.

Bayesian Filters

We need to build a model which given a known position, can tell us where the robot will be given a known velocity command.

For example, without any additional computation:

$$\begin{array}{rl}{x}_{t-1}& =⟨3,2⟩\\ u_t& =⟨1,0⟩\\ x_t& =f(u_t,x_{t-1})\\ & =⟨2,2⟩\end{array}$$

We could just use the commands to predict where the robot is but this introduces uncertainty because we don’t have any way to confirm we moved how we intended to.

Instead, we can have the distribution over possible positions for the robot and then apply the velocity command to the set to produce a new set of possible positions.

$$\begin{array}{rl}\text{let }& p(x_{t-1})\text{ be the distribution over possible positions}\\ u_t& =⟨1,0⟩\\ p(x_t)& =f(u_t,p(x_{t-1}))\end{array}$$

Every time the robot moves, there will be some error introduced:

This means the new distribution over possible positions must also itself be a distribution over possible positions taking into account any error we introduced.

$$p(x_t)=p(x_t|u_t,x_{t-1})$$

Bayes Filter

We calculate the prediction by multiplying the two distributions:

$$\hat{p}(x_t)=\int p(x_t|u_t,x_{t-1})\cdot p(x_{t-1}).d{x}_{t-1}$$

But using this formula, we would find that the variance of the distribution would keep increasing as the cumulative error compounds.

We use the measurement update to re-weight the distribution using perception:

$$p(x_t)=n\cdot p(z_t|x_t)\cdot \hat{p}(x_t)$$

$n$ is a normalisation factor which makes sure the distribution remains within 0..1 (we can ignore this for now)

$p(z_t|x_t)$ is the probability the robot will perceive something at a certain point

(see Probabilistic Robotics for example calculations)

This is all implemented in amcl_base.

Transformations

Inverse of rotation matrix is the transpose. (rotation matrices are orthogonal matrices)

ROS TF

We can show all the published transformations by running:

rosrun rqt_tf_tree rqt_tf_tree

We use the tf2 package for transformations. If we want to do transformations in python, we import:

import tf2_ros, tf_conversions

We also need to depend on tf2_ros and tf_conversions in package.xml.

For example, if we want to publish the turtle’s Pose as a transform in respect to the map:

from geometry_msgs.msg import TransformStamped
from turtlesim.msg import Pose
 
def pose_cb(pose: Pose):
	broadcaster = tf2_ros.TransformBroadcaster()
	message = TransformStamped()
	
	message.header.stamp = rospy.get_rostime()
	message.header.frame_id = 'map'
	message.child_frame_id = 'turtle1'
	
	message.transform.translation.x = pose.x
	message.transform.translation.y = pose.y
	
	quat = tf_conversions.transformations.quaternion_from_euler(0, 0, pose.theta)
	# we can skip x, y for the 2D simulation
	message.transform.rotation.z = quat[2]
	message.transform.rotation.w = quat[3]
	
	broadcaster.sendTransform(message)

Listener: