# Good algorithm for processing positional estimates

We (my team) are building a robot which will navigate around an arena.

The robot uses a camera to determine its position based on markers on the wall. We have tested this and found it can determine position well when close to the wall. However, at further distances, the markers appear to move randomly by single pixels on the sensor (due to vibration, sensor noise and other factors), causing the positional estimates to vary considerably.

Initially we took a mean of the captured samples, but if we have a list of samples like this:

(4.45, 5.06)
(4.43, 5.07)
(4.42, 6.48)
(3.95, 5.67)


then the outliers considerably change the averaged result, making the calculated position inaccurate. So, we need to remove these data points - either replace the values with more reasonable estimates, or mark them as outliers and ignore them while taking the mean. Ideally, any such algorithm should be capable of handing only a few data points, as we will only be able to see a maximum of 7 markers at any one time, and typically only 4-5.

I took GCSE Statistics about 3 years ago (and got an A) and I vaguely remember an algorithm which did just this - but my knowledge has since faded.

-
For a long list of approaches to finding outliers in multidimensional datasets, please read stats.stackexchange.com/questions/213. Your question is specialized (to 2D, small numbers of points, and has prior information about typical amounts of variation) and therefore is amenable to some special solutions not appearing in that list, but it might give you some useful ideas to get started. –  whuber Mar 19 '12 at 22:54
@whuber Thanks for your comment. I've considered some of those answers, but they're way over my head. I am looking for a simple answer, if that's even possible. I'm a programmer and engineer - mathematics is a tool, but not so much a skill for me :(. Thanks. –  Thomas O Mar 22 '12 at 21:53
Since you have a robot navigating, presumably you have continuous measurements over time, right? Why don't you use a Kalman filter? –  Memming Mar 26 '12 at 1:27

It sounds like you're looking for a robust measure of central tendency, of which there are many. The Wikipedia page on robust statistics is probably worth a read. The simplest approach is probably to use the median of the samples, rather than the mean; you'll have to do this calculation for $x$ and $y$ coordinates separately.

-
I agree. If you want a simple answer, just use the median. All this is way way over my head but if the problem really does boil down to outliers pulling the estimate, then just use the median which is not sensitive to outliers. –  Will Mar 23 '12 at 2:50

Sounds like a fun problem you're working on.

I guess a question is whether the sensors are making errors serially, or whether the errors (especially big ones) are independently distributed. Is it actually posing a problem for your robot?

I wrote a little test (below) which compares using a mean, median, weighted mean (using student's t probability of a t score from the observation mean), weighted mean (using the normal probability of a z score from the observation, which should punish outliers more), a trimmed mean, and a geometric mean. The test shows (unsurprisingly) that the arithmetic mean is most efficient in large samples, though not necessarily the best in individual samples with outliers. So again it comes to the question of the distribution of your outliers. If they're not serially correlated for a particular sensor then perhaps a good work-around is to base less of the robot's steering on the current observation, but perhaps on a couple of past observations also.

Anyhow, here's my R code, which gives you the RMSE for the six different measures. The weighting is pretty ad-hoc, but I've had too much wine to think about it too much.

Cheers, Jim

library(psych)
# Real parameter = 4.45
compare <- function(){

param <- 4.45

# standard deviation = 0.3

sd <- 0.3

# generate 1000 observations of the param

obs <- param + sd * rnorm(1000,0,1)

smpl.sd <- sd(sample(obs, 30, replace = TRUE))

# Now we randomly sample five observations, and try several different methods,
# finally comparing the different methods in terms of their RMSE.
smpl.mean <- rep(NA, 100)
smpl.med <- rep(NA, 100)
smpl.wmt <- rep(NA, 100)
smpl.wmp <- rep(NA, 100)
smpl.tm <- rep(NA, 100)
smpl.gm <- rep(NA, 100)

for(i in 1:100){
draw <- sample(obs, 5,replace = TRUE)
smpl.mean[i] <- mean(draw)
smpl.med[i] <- median(draw)
t <- (draw - smpl.mean[i])/(smpl.sd/sqrt(length(draw)))
p.t <- ifelse(t>0, pt(t,df = length(draw)), pt(t,df = length(draw), lower.tail = FALSE))
smpl.wmt[i] <- weighted.mean(draw, p.t)
z <- (draw - smpl.mean[i])/smpl.sd
p.z <- ifelse(z>0, pnorm(z), pnorm(z, lower.tail = FALSE) )
smpl.wmp[i] <- weighted.mean(draw, p.z)
smpl.tm[i] <- mean(sort(draw)[2:4])
smpl.gm[i] <- geometric.mean(draw)
}

working <- sqrt((cbind(smpl.mean, smpl.med, smpl.wmt, smpl.wmp, smpl.tm, smpl.gm)-param)^2)

rmse <- colMeans(working)
return(rmse)
}

rmses <- array(NA, c(1000,6))

for (i in 1:100){
rmses[i, ] = t(compare())
}
colMeans(rmses)

-