# The uncertainty is big while the sensor is rather accurate at measuring a landmark in EKF-SLAM

I've a 2D sensor which provides a range $r$ and a bearing $\phi$ to a landmark. In my 2D EKF-SLAM simulation, the sensor has the following specifications $$\sigma_{r} = 0.01 \text{m} \ \ ,\sigma_{\phi} = 0.5 \ \text{deg}$$

The location of the landmark in x-axis is 30. EKF imposes the Gaussian noise, therefore the location of the landmark is represented via two quantities namely the mean $\mu_{x}$ and the variance $\sigma_{x}$. In the following graph The green is the mean $\mu_{x}$ which is very close to the true location (i.e. 30). The black is the measurements and red is $\mu_{x} \pm 3 \sigma_{x}$. I don't understand why the uncertainty is big while I'm using rather accurate sensor. The process noise for the robot's pose is $\sigma_{v} = 0.001$ which is small noise. I'm using C++.

Edit: for people who ask about the measurements, this is my code

$$r = \sqrt{ (m_{j,y} - y)^{2} + (m_{j,x} - x)^{2}} + \mathcal{N}(0, \sigma_{r}^{2}) \\ \phi = \text{atan2} \left( \frac{m_{j,y} - y}{m_{j,x} - x} \right) + \mathcal{N}(0, \sigma_{\phi}^{2})$$

std::vector<double> Robot::observe( const std::vector<Beacon>& map )
{
std::vector<double> Zobs;

for (unsigned int i(0); i < map.size(); ++i)
{
double range, bearing;

range = sqrt( pow(map[i].getX() - x,2) +
pow(map[i].getY() - y,2)   );

// add noise to range
range += sigma_r*Normalized_Gaussain_Noise_Generator();

bearing = atan2( map[i].getY() - y, map[i].getX() - x) - a;

// add noise to bearing
bearing += sigma_p*Normalized_Gaussain_Noise_Generator();

bearing = this->wrapAngle(bearing);

if ( range < 1000 ){
// store measurements (range, angle) for each landmark.
Zobs.push_back(range);
Zobs.push_back(bearing);
//std::cout << range << " " << bearing << std::endl;
}
}

return Zobs;
}


where Normalized_Gaussain_Noise_Generator() is ( i.e. $\mathcal{N}(0, 1) )$

double Robot::Normalized_Gaussain_Noise_Generator()
{
double noise;
std::normal_distribution<double> distribution;
noise = distribution(generator);

return noise;
}


For the measurements (i.e. the black color), I'm using the inverse measurement function given the estimate of the robot's pose and the true measurement in polar coordinates to get the location of a landmark. The actual approach is as follows

$$\bar{\mu}_{j,x} = \bar{\mu}_{x} + r \cos(\phi + \bar{\mu}_{\theta}) \\ \bar{\mu}_{j,y} = \bar{\mu}_{y} + r \sin(\phi + \bar{\mu}_{\theta})$$

This is how it is stated in the Probabilistic Robotics book. This means that the measurements in the above graph are indeed the predicted measurements not the true ones.

Now under same conditions, the true measurements can be obtained as follows

$$\text{m}_{j,x} = x + r \cos(\phi + \theta) \\ \text{m}_{j,y} = y + r \sin(\phi + \theta)$$

The result is in the graph below, which means there is no correlations between the true measurements and the robot's estimate. This leads me to the same question - why the uncertainty behaves like that? You don't describe your setup in detail, and some of the units are missing, but my guess is that the $\sigma_\phi$ is mainly responsible for the initial error you are seeing.

$\sin( 0.5 ) \times 30 \approx 0.261$

• the unit of the angle is radian in the code. Feb 25, 2015 at 20:28
• it says deg in your question. Feb 26, 2015 at 11:00

Something is wrong with your code. Measurements with Gaussian noise should be within one standard deviation of truth 68% of the time. Somehow, your measurements seem to be correlated with the accuracy of your estimate.

Go through the code or post some here.

• Reverse that: The accuracy of the estimate is correlated with the measurements, and it sounds absolutely correct. Feb 25, 2015 at 15:59
• Whoops! I was just trying to say that measurement error statistics shouldn't change with time. Feb 25, 2015 at 16:34
• I agree. There is clearly something wrong with the measurements being received. Feb 25, 2015 at 16:34
• I have posted the measurement code with the mathematical equations. Feb 25, 2015 at 20:39
• What else you suspect in code that I need to provide. Feb 25, 2015 at 20:48

You may have covariance collapse. You may have such small covariance in your target estimate that the measurements are having almost no effect on the target's estimate. Try artificially inflating it.

It would make sense given that it "freezes" when your covariance in your target shrinks. But still, I'm hesitant to say this is the issue, since the red lines imply decent target estimate uncertainty remains.