Localization over heterogeneous features, part 2

Localization under different environments, part 2

This is a continuation of the previous post on localization under heterogenous features (see here), where we made clear that localization appears faster with more distinctive features present in an environment. The assumption of course is that the robot sensor is capable of distinguishing all of these diverse features (i.e., colors) even if the sensor is not perfectly accurate.

Let’s make this analysis more definitive by doing away with our visual grid color interpretation. It is often hard to tell which of two similar cells is more ‘white-hot’, so let’s use the actual probabilities. We start with another simulation, this time over a 50x50 gridmap. We will run four different levels of feature diversity (2, 4, 8, and 16 objects in the environment), but only show the two extremes. Below are the simulations:

Probability plot

It is obvious that the probability assigned to the actual cell location is higher (more white-hot) than those from other locations as more object types are added to the environment. We should be able to confirm this by plotting the predicted probabilities at the actual cell location over several iterations for each run. Here’s the graph:

As predicted, we do see that the simulation with 16 objects tends to have higher probabilities calculated for the actual cell location at each iteration.

However, we should also compare the cell probability with the maximum probability possible for each iteration. There might be cells with even higher probabilities, which would lead us to conclude that those cells are the actual location(s). Let's explore this in the next graph.

Probability vs max probability

Let’s look at the ratio of the probability of the actual grid location against the maximum probability for any cell at each step. Here’s the graph:

From the graph above, it is hard to conclude that the 16-object example has any particular advantage. It does show that the simulations with more objects tend to reach a ratio of 1.0 quickly, suggesting that the probability at the actual cell location is calculated to be at least equal to the maximum probability possible during that iteration. But we do not know how many cells have this maximum value. In other words, we might have multiple 'best' candidate locations, and the actual cell location is only one of them.

Perhaps we are not looking at the correct ratio. If we think about it, we know that the 16-object simulation is better because the number of cells that are visually comparable to the color intensity of the actual cell is minimized. This means that the probability of the predicted location will tend to be higher compared to the average over the entire grid. Similarly, the predicted cells have very high contrast relative to neighboring cells. They are also highly concentrated, with just a few candidate locations, suggesting high predictive precision. This implies that we can apply some ratio using the cells that outperform the probability assigned to the actual cell location. Let's explore these thoughts below.

Probability vs average probability

To take advantage of contrast relative to all other cells, we look at the ratio of the probability of the actual grid location against the average probability for all cells at each step (including the actual cell location). Here’s the graph:

Number of cells equal to or better than actual location probability

As the localization gets more accurate, the number of candidate cell locations tend to decrease. With fewer cells identified as possible locations, the probabilities for all cells rise as a result. We can therefore count the number of high probability location and take the inverse. The higher the value of this ratio, the better. Its maximum value is 1.0 (100%). Let’s call this current-to-best-cells ratio:

Ratio of number of best predictions against all possible cells

We could also calculate a related ratio, where we calculate the ratio between the number of high probability cell locations and the total number of grid cells in the map. As the localization become more precise, the numerator will tend to decrease, causing the ratio to decrease to 1 over the total number of cells in the map. This is the same as the above, but expressed inversely and normalized with the total map size. Let’s call this best-cells-to-total ratio:

From the two ratios, we are able to extract the best (lowest/highest) localization with the 16-object simulation. We can also observe that localization tends to be faster with more objects. This is indicated by the earlier jump of the current-to-best-cells ratio on the 16-object run, followed by the 8-object run, and so on. There is also a limit to the best localization possible with fewer object types. This makes sense because if we have only a few features types, it would be difficult to pin down an exact location.

Let's put all these graphs together for easier comparison:

Closing thoughts

It is now very clear that the more heterogeneous the environment, assuming the algorithm is able to sense the disparate objects within such environment, the faster the localization algorithm would settle on a few cells, assigning far higher probabilities than the rest of the cells. We also observed that it is also more accurate, as shown by the maximum ratio reached by a 2-object simulation vs a 16-object simulation. The implication is that the versatility of the sensor used and the variety of objects detectable by such sensor in a particular environment are key to a fast and accurate localization.

Localization over heterogeneous features, part 1

Localization under different environments

In the previous post (here), we introduced a simple demonstration for discrete localization. Some of the examples did not clearly pinpoint a single location for the robot after the first 20 steps, although it did eliminate some locations as highly unlikely. With more iterations, the algorithm should be able to narrow down the possible locations to a few coordinates, perhaps even the actual robot location. Most of the incorrect location guesses were due to the imperfect sensor readings and the unreliable robot motion, modeled as probabilistic locations for each intended movement. In this post, we will evaluate if there are environments where the localization is faster.

The effect of environmental features diversity

In the previous experiments, a major cause for the slower localization was the limited number of distinct environmental features. We used a fairly uniform densities for only three types of observations, i.e., three different colors over the gridmap. It is therefore easy to mistake one location for another given a color reading, even after matching a chain of color readings. In some cases, even when the intervening step(s) decreased the probability of a wrong candidate location, the next step would increase the probabilities again --while the probabilistic movements made the correct candidate location less precise. This caused the localization to linger over incorrect locations for a few steps.

Simulating the effects of environmental features diversity

In this post, we explore if our hunch is correct: that an environment filled with more disparate features will allow a robot to localize quickly. Intuitively, this should be the case. A diverse environment allows a single observation to eliminate more candidate locations than would be possible in a more homogenous environment. In our simulation, if each cell was a different color, then a color reading would eliminate all but one cell, even with noisy robot motion, leading us to the correct cell location. If the color sensor is imprecise, the correct cell would still rise above its incorrect neighbors in a few steps.

We will model several environments using the same algorithm used in the previous post. We will vary the number of distinct features from 2, 4, 8, and 16. We can see the effect of these settings in the colors displayed on the grid map.

Demo 1a, all directions allowed, number of distinct features: 2

Demo 1b, all directions allowed, number of distinct features: 4

Demo 1c, all directions allowed, number of distinct features: 8

Demo 1d, all directions allowed, number of distinct features: 16

Let's also run several simulations on a left-to-right motion, as comparison to the previous post's experiments. We will also vary the diversity of features.

Demo 2a, left-to-right only, number of distinct features: 2

Demo 2b, left-to-right only, number of distinct features: 4

Demo 2c, left-to-right only, number of distinct features: 8

Demo 2d, left-to-right only, number of distinct features: 16

Closing thoughts

With these experiments, we established that the discrete localization algorithm can localize more quickly when the environment is more heterogenous. This also suggests that sensors that can detect different features are more useful in localizing than less discerning sensors, even when the latter are more accurate.

There are other experiments to explore. For example, does noisy motion affect localization in a diverse environment? In initial tests, in a highly chaotic map with small islands of cells (one or a couple of pixels) of the same color, a noisy motion leads to a harder time localizing, since practically any color could be reached in each step, so every cell tend to have some non-trivial probability. We will not explore this in detail at this time.

We could also model clusters of colors to reflect imprecise maps. In human terms, when we are inside a room, we can estimate that we are closer to one wall vs an opposing wall, but we would have to estimate if we are one fourth of the way, but would not know exactly (and we could be one-third or one-fifth closer to the wall). We can model this with color clusters in our map. Our intuition is that the probability will spread out across these clusters, but becoming more accurate as the robot detects a color change when traversing a cell cluster border. This is similar to being lost at sea without visible landmarks, but at least knowing you are in one of several island in the area when you get to any shore. This is indeed the case in initial experiments, which we will not pursue in more detail at this time.

Discrete localization demo

Simulated robot localization demo

My previous posts have been about machine learning techniques. Let’s do something different and get into non-ML AI this time, into the world of robotics.

Save a lost friend

Imagine trying to locate a friend lost in unrecognizable terrain with no distinct landmarks. Your friend has an altimeter on his phone, but no GPS or any other navigational tool. His phone can send text/call otherwise. You are trying to find him by using a contour map. He can give you the elevation at his current location, which he does every minute while resting. Being smart, you thought it would be easier to ‘guess’ her location if he walks towards the sun. You are unsure about his exact hiking speed, and hence his exact displacement/location after he walks for each minute between his elevation updates. But you have a good estimate of how much farther a normal person would be when walking in this terrain per minute –as a proxy for your friend's walking abilities– including estimates of minor deviations away from an intended straight line walking path. Can you estimate his location?

A simple solution…

Logic and a little math tell us we can do this. We simply need to match the elevation updates with possible routes that point towards the sun. We could start with all elevations in the contour map that match her first elevation status. Then eliminate the ones that do not match the second elevation status along lines that point towards the sun. We continue doing this for a series of elevation updates, and after some time, as long as she goes through elevation changes now and then as she walks, we should be able to eliminate all but one location.

… complicated by uncertainties

But how do we handle the uncertainty introduced by the walking estimates, the reliability of the altimeter, or the accuracy of the map itself? The solution remains the same. We simply use more probabilities. And we can use AI techniques to solve this. Note that this is not strictly an AI problem, but it is a basic problem in AI and robotics.

Robot localization

This example is similar to how robots orient themselves in a known environment. The robot in our analogy has a map and a sensor. It knows where it is going directionally. Its first job is to locate itself in the map. This is called localization. It has to keep on estimating its location as it moves through this environment while accounting for tiny but accumulating errors in its movement, including possible sensor errors because sensors in the real world are not perfect. The best situation is a perfect sensor and exact motion calculations, but in reality these are always inaccurate. Optical sensors might be muddied or affected by available light sources. Movement is inexact because a motor command is translated with errors, perhaps because the motor is running out of juice, or wearing out, the wheels are slipping against the ground, and son on. In fact, even if we give the robot its precise starting coordinates, and work from there, we would still likely lose track of its next location due to these errors. The good thing is that they can be empirically estimated well enough to narrow our search.

Localization simulation

Below are an examples of how this could be done. The first graph is the location of the robot. The second is the current estimate of possible locations, shown as a heat map where high temperature (white-hot) means higher probability for that cell to be the robot location. The third graph is the environment, represented as a grid map of different colors. The fourth graph shows what the single sensor detects (the color of the current location). The fourth graph does not provide any X-Y coordinates to the localization algorithm. We just show the coordinates to help confirm that it matches the gridmap.

This example simulates a ‘robot’ that has wheels that are unreliable. For every movement, it could end up in different neighboring cell locations. It might not even move, or it might veer off, with different amounts of probability (which I assign pre-run). The robot's movement is randomly generated, with a bias towards continuing its current trajectory (not immediate backtracking, to make the localization faster). The variety of colors in the environment is also randomly generated, via a pre-run setting on color densities. The gridmap loops around itself in all directions, a common practice in simple simulations to minimize bounds checking code. This simplication sometimes prevents the algorithm from settling on a single stable localization.

The movement probabilities is given as P(move) in the graph, where the array P(move) is [current location; one cell forward; two cells forward; three cells forward; one cell forward, veer left; one cell forward, veer right; two cells forward, veer left; two cells forward, veer right].

We also model an imperfect color sensor. For these runs, we keep it as 0.9 --that is, for each color reading, it is 90% likely that the sensor detected the correct color, and a 10% likelihood that it failed and the cell is in fact a different color. Intuitively, we expect that the robot with an imperfect sensor will have a harder time finding itself accurately if neighboring cells have different colors. The worse the sensor, the harder the search.

The simulations below are limited to approximately 20 steps to minimize the GIF file size. Each step is made up of two sub-steps: a sensing step where the robot determines the color of its current grid cell, and a movement step where the robot moves based on a pre-generated random sequence. Notice that the probabilities become more precise with each color sensing step as the robot determines more exactly where it might be, and become less precise with each movement, because the robot has to assume multiple locations due to its 'faulty' wheels.

Demo 1A: box map, multi-directional movement, probabilistic motion, seed=0

The first demo allows the robot to move in any of four directions (left, right, up, down).

Demo 1B: box map, multi-directional movement, probabilistic motion, seed 1

The second demo is similar to the first, but with the robot following a different set of randomly generated movements.

Demo 1C: rectangle map, left-to-right movement only, seed 0

The third demo below restricts the robot to move only from left-to-right.

Demo 1D: rectangle map, left-to-right movement only, seed 1

The fourth demo is similar to the third, but with a different initial robot location.

Localization theory

This is based on the Udacity/Georgia Tech lecture on localization (the Markov/discrete variety, the simplest kind of localization), under AI for Robotics taught by Prof. Sebastian Thrun. He is famous for Stanley, the Stanford-entered autonomous car that won the DARPA Grand Driving Challenge in 2005. Thrun went on to work on Google’s self-driving car effort. His work, along with those of competing teams, became the lynchpin for today’s autonomous driving cars.

The theory is actually as simple as we described it above. It is a series of measurements and updating probabilities until most locations are eliminated as unlikely. As the robot moves, we just spread out to its possible locations. As we stop and sense the color of the current grid, we then update probabilities, penalizing those that do not match and increasing those that do. In theory, the location estimates are adjusted based on pre-established movement probabilities. In other words, if a robot was given an order to move its motors, there is an assumption of its range of possible motion/translation in the real world. This could also be fully replaced by a step to read inertial sensors, then build the probabilities based on the empirical probabilities of these inertial sensors. In other words, it will just be a series of sensor readings: read color sensor, read inertial sensor, read color sensor, and so on, independent of the motor commands.

Experiments with more precision

With better mechanicals and motion controls, we can make the robot movement more precise. In our simulation, let’s presume that we now have a robot that perfectly goes straight (no left-right deviations), but still has some errors with random wheel slippage. We can model this below. Notice that the spread of probabilities on each motion step is now purely along the axis of motion. We expect that we can find the robot more quickly than in the above example, and we do. It might be a little bit difficult to notice the color contrasts on the heatmap, but the algorithm can in fact calculate the location probabilities more precisely at each step.

The simulations below were done on exactly the same settings as those above, except for the P(move) probabilities, as shown in the graph title. We still have motion unreliability, but we restrict it to 0-3 cells directly in front of the robot.

Demo 2A: no side slip, box map, multi-directional movement, seed 0

Demo 2B: no side slip, rectangle map, left-to-right movement only, seed 0

Demo 2C: no side slip, small map A, left-to-right movement only, seed 0

Demo 2D: no side slip, small map B, left-to-right movement only, seed 0

This could be easily adopted –or rather I am very curious if it could be adopted easily-- to actual robots running over colored tiles, i.e., LEGO Mindstorms, which I will leave as a future project. We can also appreciate that this can be easily scaled to more sophisticated sensors and maps, e.g., a simple non-GPS drone navigation system using a local overhead map, and the sensor probabilities can be based on matching map features taken by a drone camera.

Closing thoughts

With the demo code that generated the above demo animation, I tested and produced interesting results on the accuracy tradeoffs between the two sensor probabilities (the color detector vs. the motion detector), and how the localization prediction behaves under different color densities and number of colors. We will not explore these results here as there are more interesting things to pursue on discrete localization.