Initial commit

23 files changed, 1962 insertions, 0 deletions

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..ac06689
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,5 @@
+.DS_Store
+.idea
+*.log
+tmp/
+__pycache__
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..38f3651
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,202 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(murl)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES murl
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/murl.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/murl_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_murl.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/launch/all-robots.launch.in b/launch/all-robots.launch.in
new file mode 100644
index 0000000..d98782a
--- /dev/null
+++ b/launch/all-robots.launch.in
@@ -0,0 +1,22 @@
+<launch>
+  <!--
+       Default GUI to true for local Gazebo client. Allows override
+       to set disable GUI for RoboMaker. See part0.launch.
+  -->
+  <arg name="use_gui" default="true"/>
+  <arg name="model" default="burger" doc="model type [burger, waffle, waffle_pi]"/>
+  <arg name="world" default="simple" doc="world [simple]"/>
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find murl)/worlds/$(arg world).world"/>
+    <arg name="paused" value="true"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="$(arg use_gui)"/>
+    <arg name="headless" value="false"/>
+    <arg name="debug" value="false"/>
+  </include>
+
+  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
+
+  <!-- INCLUDE ROBOTS -->
+</launch>
diff --git a/launch/localisation.launch b/launch/localisation.launch
new file mode 100644
index 0000000..9aa9447
--- /dev/null
+++ b/launch/localisation.launch
@@ -0,0 +1,9 @@
+<launch>
+	<arg name="robots" />
+    <arg name="cloud" />
+	<node pkg="murl"
+          type="localisation.py"
+          name="localisation"
+          args="--robots=$(arg robots) --cloud=$(arg cloud)"
+          output="screen"/>
+</launch>
diff --git a/launch/one-robot.launch b/launch/one-robot.launch
new file mode 100644
index 0000000..74ed60c
--- /dev/null
+++ b/launch/one-robot.launch
@@ -0,0 +1,12 @@
+<launch>
+  <arg name="robot_name"/>
+  <arg name="x_pos"/>
+  <arg name="y_pos"/>
+  <arg name="z_pos"/>
+
+  <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model $(arg robot_name) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param /robot_description" />
+
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+    <param name="publish_frequency" type="double" value="50.0" />
+  </node>
+</launch>
diff --git a/launch/rviz0.launch b/launch/rviz0.launch
new file mode 100644
index 0000000..b9028cb
--- /dev/null
+++ b/launch/rviz0.launch
@@ -0,0 +1,13 @@
+<launch>
+  <arg name="model" default="burger" doc="model type [burger, waffle, waffle_pi]"/>
+
+  <!--
+       Default -Open rViz config to true for local rviz client.
+       Allows override to disable it for RoboMaker. See hello_world_aws.launch.
+  -->
+  <arg name="open_rviz_config" default="true"/>
+  <group if="$(arg open_rviz_config)">
+    <node name="rviz0" pkg="rviz" type="rviz" args="-d $(find murl)/rviz/robot0.rviz"/>
+  </group>
+
+</launch>
diff --git a/launch/rviz1.launch b/launch/rviz1.launch
new file mode 100644
index 0000000..3ce0ab8
--- /dev/null
+++ b/launch/rviz1.launch
@@ -0,0 +1,13 @@
+<launch>
+  <arg name="model" default="burger" doc="model type [burger, waffle, waffle_pi]"/>
+
+  <!--
+       Default -Open rViz config to true for local rviz client.
+       Allows override to disable it for RoboMaker. See hello_world_aws.launch.
+  -->
+  <arg name="open_rviz_config" default="true"/>
+  <group if="$(arg open_rviz_config)">
+    <node name="rviz1" pkg="rviz" type="rviz" args="-d $(find murl)/rviz/robot1.rviz"/>
+  </group>
+
+</launch>
diff --git a/localisation.py b/localisation.py
new file mode 100755
index 0000000..c664f72
--- /dev/null
+++ b/localisation.py
@@ -0,0 +1,172 @@
+#!/usr/bin/env python3
+import argparse
+import copy
+import importlib
+import time
+
+import numpy as np
+import rospy
+
+from geometry_msgs.msg import Twist
+from numpy.random import default_rng
+from std_msgs.msg import Header
+
+import murl.sensors as sensors
+from murl.world.simple import SimpleWorld
+
+
+RNG = default_rng()
+
+
+class Robot:
+    SENSORS_TO_WHEELS = np.array([[0.2, 0.2, -0.2, 0.1, -0.1],
+                                  [0.2, -0.2, 0.2, -0.1, 0.1]])
+    WHEELS_TO_UW = np.array([[.5, .5], [.5, -.5]])
+    SENSORS_TO_UW = np.matmul(WHEELS_TO_UW, SENSORS_TO_WHEELS)
+
+    def __init__(self, world, robot, cloud_module):
+        self._robot = robot
+        self._publisher = rospy.Publisher('/robot{}/cmd_vel'.format(robot), Twist, queue_size=5)
+        self._laser = sensors.SimpleLaser(robot)
+        self._motion = sensors.Motion(robot)
+        self._groundtruth = sensors.GroundtruthPose(robot)
+        self._particles = cloud_module.Cloud(world, robot, RNG)
+        self._particles.resample_all()
+
+
+    def move(self):
+        f, fl, fr, l, r = self._laser.measurements
+        inputs = np.clip(np.array([f, fl, fr, l, r]), 0, 3.0)
+        uw = np.matmul(self.SENSORS_TO_UW, inputs)
+
+        u = uw[0]
+        w = uw[1]
+
+        if f < 0.3:
+            u = -0.01
+            w = 0.5
+        elif fl < 0.5:
+            u = -0.01
+            w = -0.5
+        elif fr < 0.5:
+            u = -0.01
+            w = 0.5
+        elif l < 0.5:
+            w = -0.5
+        elif r < 0.5:
+            w = 0.5
+
+        msg = Twist()
+        msg.linear.x = u
+        msg.angular.z = w
+        self._publisher.publish(msg)
+
+
+    @property
+    def ready(self):
+        return self._laser.ready and self._motion.ready and self._groundtruth.ready
+
+
+    @property
+    def delta_pose(self):
+        return self._motion.delta_pose
+
+
+    @property
+    def pose(self):
+        return self._groundtruth.pose
+
+
+    @property
+    def scan(self):
+        return self._laser._measurements
+
+
+    @property
+    def particles(self):
+        return self._particles
+
+
+def run(args):
+    rospy.init_node('localisation')
+
+    # Update control every 100 ms.
+    rate_limiter = rospy.Rate(100)
+
+    world = SimpleWorld(RNG)
+    cloud_module = importlib.import_module(args.cloud)
+    robots = [Robot(world, i, cloud_module) for i in range(args.robots)]
+    robot_guesses = [None for i in range(args.robots)]
+    robot_scans = [[None for i in range(args.robots)] for i in range(args.robots)]
+
+    for i, robot in enumerate(robots):
+        robot_guesses[i] = robot.particles.guess_position()
+    print('Ready!')
+
+    pose_history = []
+    with open('/tmp/gazebo_exercise.txt', 'w'):
+        pass
+
+    frame_id = 0
+    while not rospy.is_shutdown():
+        # Make sure all measurements are ready.
+        if not all(map(lambda robot: robot.ready, robots)):
+            rate_limiter.sleep()
+            continue
+
+        # Update robot positions
+        for robot in robots:
+            robot.move()
+
+
+        # Recalculate relative robot positions
+        for i in range(args.robots):
+            for j in range(args.robots):
+                if i == j:
+                    continue
+                dist = robots[j].pose[:2] - robots[i].pose[:2]
+                angle = np.arctan2(dist[1], dist[0]) - robots[i].pose[2]
+                robot_scans[i][j] = np.sqrt(dist[0]**2 + dist[1]**2), angle
+
+        for i, robot in enumerate(robots):
+            delta = robot.delta_pose
+            angle_dists = zip(sensors.SCAN_ANGLES, robot.scan)
+            robot.particles.move(delta)
+            guesses = robot_guesses[:i] + robot_guesses[i+1:]
+            scans = robot_scans[i][:i] + robot_scans[i][i+1:]
+            robot.particles.calculate_weight(angle_dists, guesses, scans)
+
+
+        # Resample particles and update guesses
+        for i, robot in enumerate(robots):
+            if frame_id % 10:
+                robot.particles.publish(frame_id)
+            robot.particles.average()
+            robot_guesses[i] = robot.particles.guess_position()
+
+        # Log positions
+        pose_history.append([(robot.pose[:2], guess[0]) for robot, guess in zip(robots, robot_guesses)])
+        if len(pose_history) % 10:
+            def entry_to_string(per_robot):
+                return ','.join(','.join(str(v) for v in robot) + ',' + ','.join(str(v) for v in guess) for robot, guess in per_robot)
+
+            with open('/tmp/data.csv', 'a') as fp:
+                fp.write('\n'.join(entry_to_string(entry) for entry in pose_history) + '\n')
+
+            pose_history = []
+
+        # Loop again
+        rate_limiter.sleep()
+        frame_id += 1
+
+
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser(description='Runs a particle filter')
+    parser.add_argument('--robots', action='store', type=int, help='Number of robots.')
+    parser.add_argument('--cloud', action='store', type=str, help='Particle cloud module.')
+    args, unknown = parser.parse_known_args()
+    try:
+        # cProfile.run('run(args)', '/tmp/profile-stats')
+        run(args)
+    except rospy.ROSInterruptException:
+        pass
diff --git a/models/simple_world/model.config b/models/simple_world/model.config
new file mode 100644
index 0000000..c5582f1
--- /dev/null
+++ b/models/simple_world/model.config
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Exercise World</name>
+  <version>1.0</version>
+  <sdf version="1.0">model.sdf</sdf>
+
+  <author>
+    <name>asp45</name>
+    <email>asp45@cl.cam.ac.uk</email>
+  </author>
+
+  <description>
+    Simple world for the exercises.
+  </description>
+</model>
diff --git a/models/simple_world/model.sdf b/models/simple_world/model.sdf
new file mode 100644
index 0000000..c28c246
--- /dev/null
+++ b/models/simple_world/model.sdf
@@ -0,0 +1,121 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='simple'>
+    <link name='fixed'></link>
+    <pose frame=''>0 0 0 0 -0 0</pose>
+    <link name='Wall_0'>
+      <collision name='Wall_0_Collision'>
+        <geometry>
+          <box>
+            <size>4.3 0.15 0.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 0.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_0_Visual'>
+        <pose frame=''>0 0 0.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>4.3 0.15 0.5</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>0 2.075 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_2'>
+      <collision name='Wall_2_Collision'>
+        <geometry>
+          <box>
+            <size>4.3 0.15 0.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 0.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_2_Visual'>
+        <pose frame=''>0 0 0.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>4.3 0.15 0.5</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>-2.075 0 0 0 0 -1.5708</pose>
+    </link>
+    <link name='Wall_3'>
+      <collision name='Wall_3_Collision'>
+        <geometry>
+          <box>
+            <size>4.3 0.15 0.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 0.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_3_Visual'>
+        <pose frame=''>0 0 0.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>4.3 0.15 0.5</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>0 -2.075 0 0 -0 0</pose>
+    </link>
+    <link name='Wall_4'>
+      <collision name='Wall_4_Collision'>
+        <geometry>
+          <box>
+            <size>4.3 0.15 0.5</size>
+          </box>
+        </geometry>
+        <pose frame=''>0 0 0.25 0 -0 0</pose>
+      </collision>
+      <visual name='Wall_4_Visual'>
+        <pose frame=''>0 0 0.25 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>4.3 0.15 0.5</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>2.075 0 0 0 -0 1.5708</pose>
+    </link>
+
+    <link name='Cylinder_1'>
+      <collision name='Cylinder_1_Collision'>
+        <geometry>
+          <cylinder>
+            <radius>0.3</radius>
+            <length>0.5</length>
+          </cylinder>
+        </geometry>
+        <pose frame=''>0 0 0.25 0 -0 0</pose>
+      </collision>
+      <visual name='Cylinder_1_Visual'>
+        <pose frame=''>0 0 0.25 0 -0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.3</radius>
+            <length>0.5</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <ambient>1 1 1 1</ambient>
+        </material>
+      </visual>
+      <pose frame=''>0.3 0.2 0 0 0 0</pose>
+    </link>
+    <static>1</static>
+  </model>
+</sdf>
diff --git a/murl/particles/__init__.py b/murl/particles/__init__.py
new file mode 100644
index 0000000..e460c10
--- /dev/null
+++ b/murl/particles/__init__.py
@@ -0,0 +1,130 @@
+from abc import ABC, abstractmethod
+
+import numpy as np
+import rospy
+
+from geometry_msgs.msg import Point32
+from sensor_msgs.msg import ChannelFloat32, PointCloud
+
+class ParticleCloud(ABC):
+    PARTICLE_COUNT = 10000
+    RESAMPLE_PROB = 2**-10
+    RAY_SAMPLES = 3
+    ALPHAS = [0.01, 0.00, 0.01, 0.00]
+
+    def __init__(self, world, robot, rng):
+        self._world = world
+        self._particles = np.array([[0.0] * 4] * self.PARTICLE_COUNT)
+        self._publisher = rospy.Publisher('/robot{}/particles'.format(robot), PointCloud, queue_size=1)
+        self._frame = 'robot{}/odom'.format(robot)
+        self._robot = robot
+        self._rng = rng
+
+
+    def resample_all(self):
+        self.resample(np.array([True] * self.PARTICLE_COUNT))
+
+
+    def resample(self, mask):
+        tmp = self._particles[mask,:]
+        self._world.sample(tmp)
+        self._particles[mask,:] = tmp
+
+
+    def move(self, delta):
+        rot1 = np.arctan2(delta[1], delta[0])
+
+        # Assume large rot1 means reversing
+        if rot1 < -np.pi:
+            rot1 += np.pi
+        elif rot1 > np.pi:
+            rot1 -= np.pi
+
+        trans = np.sqrt(delta[0]**2 + delta[1]**2)
+        rot2 = delta[2] - rot1
+
+        rot1_var = self.ALPHAS[0] * rot1**2 + self.ALPHAS[1] * trans**2
+        trans_var = self.ALPHAS[2] * trans**2 + self.ALPHAS[3] * (rot1**2 + rot2**2)
+        rot2_var = self.ALPHAS[0] * rot2**2 + self.ALPHAS[1] * trans**2
+
+        rot1 += self._rng.normal(scale = np.sqrt(rot1_var), size=self.PARTICLE_COUNT)
+        trans += self._rng.normal(scale = np.sqrt(trans_var), size=self.PARTICLE_COUNT)
+        rot2 += self._rng.normal(scale = np.sqrt(rot2_var), size=self.PARTICLE_COUNT)
+
+        self._particles[:,0] += trans * np.cos(rot1 + self.yaws)
+        self._particles[:,1] += trans * np.sin(rot1 + self.yaws)
+        self._particles[:,2] += rot1 + rot2
+
+        mask = self._rng.random(self.PARTICLE_COUNT) < self.RESAMPLE_PROB
+        self.resample(mask)
+
+
+    @abstractmethod
+    def calculate_weight(self, angle_dists, robot_guesses, robot_scans):
+        raise NotImplementedError()
+
+
+    def average(self):
+        biggest = max(self._particles[:,3])
+        mask = self._particles[:,3] > biggest - 5
+        masked = self._particles[mask]
+
+        if len(masked) < 20 or not np.isfinite(biggest):
+            print('Massive collapse for robot', self._robot)
+            self._particles[:,3] = 0.
+            self.resample_all()
+            return
+
+        weights = np.exp(masked[:,3] - max(masked[:,3]))
+        self._particles = self._rng.choice(masked, size=self.PARTICLE_COUNT, p=weights/np.sum(weights))
+        self._particles[:,3] = 0.0
+
+
+    def guess_position(self):
+        weights = np.exp(self._particles[:,3])
+        weights /= np.sum(weights)
+
+        mean = np.average(self._particles[:,:2], weights=weights, axis=0)
+        cov = np.cov(self._particles[:,:2], rowvar=False, ddof=0, aweights=weights)
+
+        # x and y being too correlated is suspicious
+        if abs(cov[0,0]*cov[1,1] - cov[0,1] * cov[1,0]) < 1e-15:
+            print('Singular covariance matrix for robot', self._robot)
+            # pretend to uncorrelate the data slightly
+            cov[0,0] += 0.1
+            cov[1,1] += 0.1
+
+        return mean, cov
+
+
+    def publish(self, frame_id):
+        msg = PointCloud()
+        msg.header.seq = frame_id
+        msg.header.stamp = rospy.Time.now()
+        msg.header.frame_id = self._frame
+        intensity = ChannelFloat32()
+        intensity.name = 'intensity'
+        msg.channels.append(intensity)
+        msg.points = [Point32(x = particle[0], y = particle[1], z = 0.05) for particle in self._particles]
+        intensity.values = list(np.exp(self._particles[:,3]))
+        self._publisher.publish(msg)
+
+
+    @property
+    def particles(self):
+        return self._particles
+
+
+    @property
+    def yaws(self):
+        return self._particles[:,2]
+
+
+    @property
+    def log_weights(self):
+        return self._particles[:,3]
+
+
+    @property
+    def weights(self):
+        return np.exp(self.log_weights)
diff --git a/murl/particles/baseline.py b/murl/particles/baseline.py
new file mode 100644
index 0000000..2d8e045
--- /dev/null
+++ b/murl/particles/baseline.py
@@ -0,0 +1,21 @@
+from murl.particles import ParticleCloud
+from murl.sensors import CONE_WIDTH
+
+import numpy as np
+from scipy.stats import norm
+
+class Cloud(ParticleCloud):
+    def calculate_weight(self, angle_dists, _robot_guesses, _robot_scans):
+        self._particles[:,3] = 0.0
+        mask = self._world.is_valid(self._particles)
+        self._particles[~mask,3] = float('-inf')
+        count = np.ma.array(self._particles[:,3], mask=~mask).count()
+
+        for angle, dist in angle_dists:
+            angles = angle + self._rng.uniform(-CONE_WIDTH/2, CONE_WIDTH/2,
+                                               (self.RAY_SAMPLES, count))
+            true_dists = self._world.get_dist(self._particles[mask], angles)
+            if np.isfinite(dist):
+                self._particles[mask,3] += norm.logpdf(dist, true_dists, 0.8)
+            else:
+                self._particles[mask,3] += norm.logsf(3, true_dists, 0.8)
diff --git a/murl/particles/centralised.py b/murl/particles/centralised.py
new file mode 100644
index 0000000..33be0b3
--- /dev/null
+++ b/murl/particles/centralised.py
@@ -0,0 +1,30 @@
+from murl.particles import ParticleCloud
+from murl.sensors import CONE_WIDTH
+
+import numpy as np
+from scipy.stats import multivariate_normal, norm
+
+class Cloud(ParticleCloud):
+    BETA = 1.E+0
+
+    def calculate_weight(self, angle_dists, robot_guesses, robot_scans):
+        self._particles[:,3] = 0.0
+        mask = self._world.is_valid(self._particles)
+        self._particles[~mask,3] = float('-inf')
+        count = np.ma.array(self._particles[:,3], mask=~mask).count()
+
+        for angle, dist in angle_dists:
+            angles = angle + self._rng.uniform(-CONE_WIDTH/2, CONE_WIDTH/2,
+                                               (self.RAY_SAMPLES, count))
+            true_dists = self._world.get_dist(self._particles[mask], angles)
+
+            if np.isfinite(dist):
+                self._particles[mask,3] += norm.logpdf(dist, true_dists, 0.8)
+            else:
+                self._particles[mask,3] += norm.logsf(3, true_dists, 0.8)
+
+        for guess, scan in zip(robot_guesses, robot_scans):
+            angles = scan[1] + self._particles[mask,2]
+            poses = scan[0] * np.stack([np.cos(angles), np.sin(angles)]).transpose() + self._particles[mask,:2]
+            update = multivariate_normal.logpdf(poses, guess[0], guess[1])
+            self._particles[mask,3] += self.BETA * update
diff --git a/murl/sensors.py b/murl/sensors.py
new file mode 100644
index 0000000..700db35
--- /dev/null
+++ b/murl/sensors.py
@@ -0,0 +1,180 @@
+import numpy as np
+import rospy
+
+from gazebo_msgs.msg import ModelStates
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import LaserScan
+from tf.transformations import euler_from_quaternion
+
+SCAN_ANGLES = [0., np.pi / 4., -np.pi / 4., np.pi / 2., -np.pi / 2.]
+CONE_WIDTH = np.pi / 180. * 3.1 # 3.1 degrees cone of view (3 rays).
+
+class SimpleLaser:
+    def __init__(self, robot):
+        rospy.Subscriber('/robot{}/scan'.format(robot), LaserScan, self.callback)
+        self._angles = SCAN_ANGLES
+        self._width = CONE_WIDTH
+        self._measurements = [float('inf')] * len(self._angles)
+        self._indices = None
+
+    def callback(self, msg):
+        # Helper for angles.
+        def _within(x, a, b):
+            pi2 = np.pi * 2.
+            x %= pi2
+            a %= pi2
+            b %= pi2
+            if a < b:
+                return a <= x and x <= b
+            return a <= x or x <= b;
+
+        # Compute indices the first time.
+        if self._indices is None:
+            self._indices = [[] for _ in range(len(self._angles))]
+            for i, d in enumerate(msg.ranges):
+                angle = msg.angle_min + i * msg.angle_increment
+                for j, center_angle in enumerate(self._angles):
+                    if _within(angle, center_angle - self._width / 2., center_angle + self._width / 2.):
+                        self._indices[j].append(i)
+
+        ranges = np.array(msg.ranges)
+        for i, idx in enumerate(self._indices):
+            # We do not take the minimum range of the cone but the 10-th percentile for robustness.
+            self._measurements[i] = np.percentile(ranges[idx], 10)
+
+    @property
+    def ready(self):
+        return not np.isnan(self._measurements[0])
+
+    @property
+    def measurements(self):
+        return self._measurements
+
+
+class Motion:
+    def __init__(self, robot):
+        self._previous_time = None
+        self._delta_pose = np.array([0., 0., 0.], dtype=np.float32)
+        rospy.Subscriber('/robot{}/odom'.format(robot), Odometry, self.callback)
+
+    def callback(self, msg):
+        u = msg.twist.twist.linear.x
+        w = msg.twist.twist.angular.z
+        if self._previous_time is None:
+            self._previous_time = msg.header.stamp
+        current_time = msg.header.stamp
+        dt = (current_time - self._previous_time).to_sec()
+        self._delta_pose[0] += u * dt
+        self._delta_pose[1] += 0.
+        self._delta_pose[2] += w * dt
+        self._previous_time = current_time
+
+    @property
+    def ready(self):
+        return True
+
+    @property
+    def delta_pose(self):
+        ret = self._delta_pose.copy()
+        self._delta_pose[:] = 0
+        return ret
+
+
+class SimpleLaser:
+    def __init__(self, robot):
+        rospy.Subscriber('/robot{}/scan'.format(robot), LaserScan, self.callback)
+        self._angles = SCAN_ANGLES
+        self._width = CONE_WIDTH
+        self._measurements = [float('inf')] * len(self._angles)
+        self._indices = None
+
+    def callback(self, msg):
+        # Helper for angles.
+        def _within(x, a, b):
+            pi2 = np.pi * 2.
+            x %= pi2
+            a %= pi2
+            b %= pi2
+            if a < b:
+                return a <= x and x <= b
+            return a <= x or x <= b;
+
+        # Compute indices the first time.
+        if self._indices is None:
+            self._indices = [[] for _ in range(len(self._angles))]
+            for i, d in enumerate(msg.ranges):
+                angle = msg.angle_min + i * msg.angle_increment
+                for j, center_angle in enumerate(self._angles):
+                    if _within(angle, center_angle - self._width / 2., center_angle + self._width / 2.):
+                        self._indices[j].append(i)
+
+        ranges = np.array(msg.ranges)
+        for i, idx in enumerate(self._indices):
+            # We do not take the minimum range of the cone but the 10-th percentile for robustness.
+            self._measurements[i] = np.percentile(ranges[idx], 10)
+
+    @property
+    def ready(self):
+        return not np.isnan(self._measurements[0])
+
+    @property
+    def measurements(self):
+        return self._measurements
+
+
+class Motion(object):
+    def __init__(self, robot):
+        self._previous_time = None
+        self._delta_pose = np.array([0., 0., 0.], dtype=np.float32)
+        rospy.Subscriber('/robot{}/odom'.format(robot), Odometry, self.callback)
+
+    def callback(self, msg):
+        u = msg.twist.twist.linear.x
+        w = msg.twist.twist.angular.z
+        if self._previous_time is None:
+            self._previous_time = msg.header.stamp
+        current_time = msg.header.stamp
+        dt = (current_time - self._previous_time).to_sec()
+        self._delta_pose[0] += u * dt
+        self._delta_pose[1] += 0.
+        self._delta_pose[2] += w * dt
+        self._previous_time = current_time
+
+    @property
+    def ready(self):
+        return True
+
+    @property
+    def delta_pose(self):
+        ret = self._delta_pose.copy()
+        self._delta_pose[:] = 0
+        return ret
+
+
+class GroundtruthPose:
+    def __init__(self, robot):
+        rospy.Subscriber('/gazebo/model_states', ModelStates, self.callback)
+        self._pose = np.array([np.nan, np.nan, np.nan], dtype=np.float32)
+        self._name = 'Robot{}'.format(robot)
+
+    def callback(self, msg):
+        idx = [i for i, n in enumerate(msg.name) if n == self._name]
+        if not idx:
+            raise ValueError('Specified name "{}" does not exist.'.format(self._name))
+        idx = idx[0]
+        self._pose[0] = msg.pose[idx].position.x
+        self._pose[1] = msg.pose[idx].position.y
+        _, _, yaw = euler_from_quaternion([
+                msg.pose[idx].orientation.x,
+                msg.pose[idx].orientation.y,
+                msg.pose[idx].orientation.z,
+                msg.pose[idx].orientation.w])
+        self._pose[2] = yaw
+
+    @property
+    def ready(self):
+        return not np.isnan(self._pose[0])
+
+    @property
+    def pose(self):
+        return self._pose
diff --git a/murl/world/__init__.py b/murl/world/__init__.py
new file mode 100644
index 0000000..254a0f6
--- /dev/null
+++ b/murl/world/__init__.py
@@ -0,0 +1,15 @@
+from abc import ABC, abstractmethod
+
+class World(ABC):
+    ROBOT_RADIUS = 0.105 / 2.
+    @abstractmethod
+    def is_valid(self, pose):
+        raise NotImplementedError
+
+    @abstractmethod
+    def get_dist(self, sources, angles):
+        raise NotImplementedError
+
+    @abstractmethod
+    def sample(self, particles):
+        raise NotImplementedError
diff --git a/murl/world/simple.py b/murl/world/simple.py
new file mode 100644
index 0000000..195c4db
--- /dev/null
+++ b/murl/world/simple.py
@@ -0,0 +1,77 @@
+import numpy as np
+
+from murl.world import World
+
+WALL_OFFSET = 2.
+CYLINDER_POSITION = np.array([.3, .2], dtype=np.float32)
+CYLINDER_OFFSET = .3
+
+SEGMENTS = list(map(lambda ps: (np.array(ps[0]), np.array(ps[1]), ps[2]),
+                    [[[-WALL_OFFSET, -WALL_OFFSET], [0, 1], 2*WALL_OFFSET],
+                     [[-WALL_OFFSET, WALL_OFFSET], [1, 0], 2*WALL_OFFSET],
+                     [[WALL_OFFSET, WALL_OFFSET], [0, -1], 2*WALL_OFFSET],
+                     [[WALL_OFFSET, -WALL_OFFSET], [-1, 0], 2*WALL_OFFSET]]))
+
+class SimpleWorld(World):
+    WALL_RADIUS = WALL_OFFSET - World.ROBOT_RADIUS
+    CYLINDER_RADIUS = CYLINDER_OFFSET + World.ROBOT_RADIUS
+
+    def __init__(self, rng):
+        self._rng = rng
+
+    def is_valid(self, poses):
+        return ((-self.WALL_RADIUS < poses[...,0])
+              & (poses[...,0] < self.WALL_RADIUS)
+              & (-self.WALL_RADIUS < poses[...,1])
+              & (poses[...,1] < self.WALL_RADIUS)
+              & (np.sum((poses[...,:2] - CYLINDER_POSITION) ** 2, axis=-1) > self.CYLINDER_RADIUS ** 2))
+
+    def get_dist(self, sources, angles):
+        def dot(x, y):
+            return x[...,0] * y[...,0] + x[...,1] * y[...,1]
+        def cross(x, y):
+            return x[...,0] * y[...,1] - x[...,1] * y[...,0]
+
+        def intersect_segment(sources, angle, start, direction, length):
+            Y = start - sources[:,:2]
+            r = np.stack((np.cos(angle + sources[:,2]),
+                          np.sin(angle + sources[:,2]))).T
+            t = cross(Y, direction) / cross(r, direction)
+            t1 = cross(Y, r) / cross(r, direction)
+
+            return np.where((t >= 0) & (t1 >= 0) & (t1 <= length), t, float('inf'))
+
+        def intersect_cylinder(sources, angle, center, radius):
+            Y = center - sources[:,:2]
+            d = np.stack((np.cos(angle + sources[:,2]),
+                          np.sin(angle + sources[:,2]))).T
+            b = dot(d, Y)
+            c = dot(Y, Y) - radius**2
+            disc = b**2 - dot(Y, Y) + radius**2
+
+            mask = (disc > 0) & (b > 0) & (b ** 2 > disc)
+
+            b[~mask] = float('inf')
+            b[mask] -= np.sqrt(disc[mask])
+
+            return b
+
+        intersections = np.stack([np.stack(
+                (intersect_segment(sources, angle, *SEGMENTS[0]),
+                 intersect_segment(sources, angle, *SEGMENTS[1]),
+                 intersect_segment(sources, angle, *SEGMENTS[2]),
+                 intersect_segment(sources, angle, *SEGMENTS[3]),
+                 intersect_cylinder(sources, angle, CYLINDER_POSITION, CYLINDER_OFFSET))
+            ) for angle in angles])
+
+        return np.amin(intersections, axis=(0,1))
+
+    def sample(self, particles):
+        particles[:,2] = self._rng.uniform(-np.pi, np.pi, size=len(particles))
+
+        invalid = np.ones(shape=len(particles), dtype=np.bool)
+        while np.any(invalid):
+            count = np.ma.array(particles[:,0], mask=~invalid).count()
+            particles[invalid,0] = self._rng.uniform(-self.WALL_RADIUS, self.WALL_RADIUS, size=count)
+            particles[invalid,1] = self._rng.uniform(-self.WALL_RADIUS, self.WALL_RADIUS, size=count)
+            invalid = ~self.is_valid(particles)
diff --git a/package.xml b/package.xml
new file mode 100644
index 0000000..6aef7ec
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,69 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>murl</name>
+  <version>0.0.0</version>
+  <description>The murl package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="gmb60@cam.ac.uk">gmb60</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/murl</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <depend>gazebo_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <exec_depend>gazebo</exec_depend>
+  <exec_depend>gazebo_ros</exec_depend>
+  <exec_depend>turtlebot3_description</exec_depend>
+  <exec_depend>turtlebot3_gazebo</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <gazebo_ros gazebo_model_path="${prefix}/models"/>
+  </export>
+</package>
diff --git a/rviz/robot0.rviz b/rviz/robot0.rviz
new file mode 100644
index 0000000..9d132ea
--- /dev/null
+++ b/rviz/robot0.rviz
@@ -0,0 +1,343 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Grid1
+        - /Axes1
+        - /LaserScan1
+        - /RobotModel1
+        - /DepthCloud1/Auto Size1
+        - /PointCloud1
+      Splitter Ratio: 0.6029411554336548
+    Tree Height: 444
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/Axes
+      Enabled: true
+      Length: 0.10000000149011612
+      Name: Axes
+      Radius: 0.009999999776482582
+      Reference Frame: <Fixed Frame>
+      Show Trail: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 0; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: robot0/scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_scan:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_back_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        wheel_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: robot0
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - Class: rviz/Camera
+      Enabled: false
+      Image Rendering: background and overlay
+      Image Topic: /camera/rgb/image_raw
+      Name: Camera
+      Overlay Alpha: 0.5
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: false
+      Visibility:
+        Axes: true
+        DepthCloud: true
+        Grid: true
+        LaserScan: true
+        Odometry: true
+        PointCloud: true
+        RobotModel: true
+        TF: true
+        Value: true
+      Zoom Factor: 1
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: false
+      Keep: 100
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Queue Size: 10
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic: /odom
+      Unreliable: false
+      Value: false
+    - Alpha: 1
+      Auto Size:
+        Auto Size Factor: 1
+        Value: true
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/DepthCloud
+      Color: 255; 255; 255
+      Color Image Topic: ""
+      Color Transformer: ""
+      Color Transport Hint: raw
+      Decay Time: 0
+      Depth Map Topic: ""
+      Depth Map Transport Hint: raw
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: DepthCloud
+      Occlusion Compensation:
+        Occlusion Time-Out: 30
+        Value: false
+      Position Transformer: ""
+      Queue Size: 5
+      Selectable: true
+      Size (Pixels): 3
+      Style: Flat Squares
+      Topic Filter: true
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 0; 255; 255
+      Min Color: 0; 255; 255
+      Name: PointCloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Spheres
+      Topic: /robot0/particles
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: robot0/odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Angle: -1.5700000524520874
+      Class: rviz/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 120.10918426513672
+      Target Frame: <Fixed Frame>
+      X: 0.006560987792909145
+      Y: 0.07163210958242416
+    Saved: ~
+Window Geometry:
+  Camera:
+    collapsed: false
+  Displays:
+    collapsed: true
+  Height: 738
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000020300000246fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003c00000246000000c800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002550000009d0000001600fffffffb0000000c00430061006d00650072006101000002cd000000f50000000000000000000000010000010f0000039bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000270000039b000000a200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005500000003efc0100000002fb0000000800540069006d00650100000000000005500000020900fffffffb0000000800540069006d00650100000000000004500000000000000000000005500000024600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1360
+  X: 284
+  Y: 0
diff --git a/rviz/robot1.rviz b/rviz/robot1.rviz
new file mode 100644
index 0000000..a75f66e
--- /dev/null
+++ b/rviz/robot1.rviz
@@ -0,0 +1,343 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Grid1
+        - /Axes1
+        - /LaserScan1
+        - /RobotModel1
+        - /DepthCloud1/Auto Size1
+        - /PointCloud1
+      Splitter Ratio: 0.6029411554336548
+    Tree Height: 759
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/Axes
+      Enabled: true
+      Length: 0.10000000149011612
+      Name: Axes
+      Radius: 0.009999999776482582
+      Reference Frame: <Fixed Frame>
+      Show Trail: false
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 0; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: robot1/scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_scan:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_back_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        wheel_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: robot1
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - Class: rviz/Camera
+      Enabled: false
+      Image Rendering: background and overlay
+      Image Topic: /camera/rgb/image_raw
+      Name: Camera
+      Overlay Alpha: 0.5
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: false
+      Visibility:
+        Axes: true
+        DepthCloud: true
+        Grid: true
+        LaserScan: true
+        Odometry: true
+        PointCloud: true
+        RobotModel: true
+        TF: true
+        Value: true
+      Zoom Factor: 1
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: false
+      Keep: 100
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Queue Size: 10
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic: /odom
+      Unreliable: false
+      Value: false
+    - Alpha: 1
+      Auto Size:
+        Auto Size Factor: 1
+        Value: true
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/DepthCloud
+      Color: 255; 255; 255
+      Color Image Topic: ""
+      Color Transformer: ""
+      Color Transport Hint: raw
+      Decay Time: 0
+      Depth Map Topic: ""
+      Depth Map Transport Hint: raw
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: DepthCloud
+      Occlusion Compensation:
+        Occlusion Time-Out: 30
+        Value: false
+      Position Transformer: ""
+      Queue Size: 5
+      Selectable: true
+      Size (Pixels): 3
+      Style: Flat Squares
+      Topic Filter: true
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 0; 255; 255
+      Min Color: 0; 255; 255
+      Name: PointCloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Spheres
+      Topic: /robot1/particles
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: robot1/odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Angle: -1.5700000524520874
+      Class: rviz/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 120.10918426513672
+      Target Frame: <Fixed Frame>
+      X: 0.006560987792909145
+      Y: 0.07163210958242416
+    Saved: ~
+Window Geometry:
+  Camera:
+    collapsed: false
+  Displays:
+    collapsed: true
+  Height: 1047
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000020300000381fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003c00000381000000c800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002550000009d0000001600fffffffb0000000c00430061006d00650072006101000002cd000000f50000000000000000000000010000010f0000039bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000270000039b000000a200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003c000000038fc0100000002fb0000000800540069006d00650100000000000003c00000020900fffffffb0000000800540069006d00650100000000000004500000000000000000000003c00000038100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 960
+  X: 0
+  Y: 768
diff --git a/scripts/analysis/error-beta.py b/scripts/analysis/error-beta.py
new file mode 100755
index 0000000..c55f240
--- /dev/null
+++ b/scripts/analysis/error-beta.py
@@ -0,0 +1,35 @@
+#!/usr/bin/env python3
+
+import matplotlib.pyplot as plt
+import numpy as np
+import pandas as pd
+import sys
+
+def process_file(filename, robots):
+    s = pd.read_csv(filename).to_numpy().reshape((-1, robots, 2, 2))
+    error = s[:,:,1] - s[:,:,0]
+    linear = np.sqrt(error[:,:,0] ** 2 + error[:,:,1] ** 2)
+    return np.median(linear)
+
+def main(filenames, labels, robots):
+    for i, names in enumerate(filenames):
+        errors = [process_file(name, robots[i]) for name in names]
+        plt.plot(labels, errors, marker='o', linestyle=' ', label='{} robots'.format(robots[i]))
+
+    plt.ylabel('Median Error (m)')
+    plt.xlabel('Significance of inter-robot ranging')
+    plt.legend()
+
+    if input('Save? ') == 'y':
+        name = input('name: ')
+        plt.savefig(name)
+
+    plt.show()
+
+if __name__ == '__main__':
+    if len(sys.argv) < 4:
+        sys.exit(1)
+    robots = [int(i) for i in sys.argv[1].split(' ')]
+    labels = sys.argv[2].split(' ')
+    filename_series = map(lambda s: s.split(' '), sys.argv[3:])
+    main(filename_series, labels, robots)
diff --git a/scripts/analysis/error-time.py b/scripts/analysis/error-time.py
new file mode 100755
index 0000000..37f0ece
--- /dev/null
+++ b/scripts/analysis/error-time.py
@@ -0,0 +1,24 @@
+#!/usr/bin/env python3
+
+import matplotlib.pyplot as plt
+import numpy as np
+import pandas as pd
+import sys
+
+def main(filename, robots):
+    s = pd.read_csv(filename).to_numpy().reshape((-1, robots, 2, 2))
+    error = s[:,:,1] - s[:,:,0]
+    linear = np.sqrt(error[:,:,0] ** 2 + error[:,:,1] ** 2)
+
+    plt.plot(linear)
+    plt.ylabel('Error (m)')
+    plt.xlabel('Time')
+    plt.show()
+
+
+if __name__ == '__main__':
+    if len(sys.argv) != 3:
+        sys.exit(1)
+    filename = sys.argv[1]
+    robots = int(sys.argv[2])
+    main(filename, robots)
diff --git a/scripts/launch-robots.py b/scripts/launch-robots.py
new file mode 100755
index 0000000..8b09e0f
--- /dev/null
+++ b/scripts/launch-robots.py
@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+
+import math
+import sys
+from typing import Tuple
+
+ORIGIN = [0.0, 0.0]
+DISTANCE = 1.5
+
+TEMPLATE = """
+  <group ns="robot{n}">
+    <param name="tf_prefix" value="robot{n}" />
+    <include file="$(find murl)/launch/one-robot.launch">
+      <arg name="robot_name" value="Robot{n}"/>
+      <arg name="x_pos" value="{x}"/>
+      <arg name="y_pos" value="{y}"/>
+      <arg name="z_pos" value="{z}"/>
+    </include>
+  </group>
+"""
+
+MAGIC_STRING = "<!-- INCLUDE ROBOTS -->"
+
+def get_pose(i : int, n : int) -> Tuple[float, float, float]:
+    angle = 2 * math.pi * i / n
+    x = ORIGIN[0] + DISTANCE * math.cos(angle)
+    y = ORIGIN[1] + DISTANCE * math.sin(angle)
+    z = 0.0
+    return (x , y , z)
+
+
+def make_robot_groups(n : int) -> str:
+    output = ""
+    for i in range(n):
+        x , y , z = get_pose(i, n)
+        output += TEMPLATE.format(
+            n = i,
+            x = round(x, 3),
+            y = round(y, 3),
+            z = round(z, 3))
+    return output
+
+
+def splice_robots(text : str, n : int) -> str:
+    return text.replace(MAGIC_STRING, make_robot_groups(n))
+
+
+def main(filename : str, n : int):
+    with open(filename, 'r') as f:
+        print(splice_robots(f.read(), n))
+
+
+if __name__ == '__main__':
+    if len(sys.argv) != 3:
+        sys.exit(1)
+    filename = sys.argv[1]
+    robots = int(sys.argv[2])
+    main(filename, robots)
diff --git a/worlds/simple.world b/worlds/simple.world
new file mode 100644
index 0000000..d5bf996
--- /dev/null
+++ b/worlds/simple.world
@@ -0,0 +1,52 @@
+<sdf version='1.6'>
+  <world name='default'>
+    <!-- A global light source -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <!-- A ground plane -->
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <physics type="ode">
+      <real_time_update_rate>100.0</real_time_update_rate>
+      <max_step_size>0.01</max_step_size>
+      <real_time_factor>0.5</real_time_factor>
+      <ode>
+        <solver>
+          <type>quick</type>
+          <iters>150</iters>
+          <precon_iters>0</precon_iters>
+          <sor>1.400000</sor>
+          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
+        </solver>
+        <constraints>
+          <cfm>0.00001</cfm>
+          <erp>0.2</erp>
+          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
+          <contact_surface_layer>0.01000</contact_surface_layer>
+        </constraints>
+      </ode>
+    </physics>
+
+    <!-- Load world -->
+    <include>
+      <uri>model://simple_world</uri>
+    </include>
+
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>false</shadows>
+    </scene>
+
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose>0.8 0.0 12.0 0 1.5708 0</pose>
+        <view_controller>orbit</view_controller>
+      </camera>
+    </gui>
+  </world>
+</sdf>