# EKF Localization when robot is in parallel with a landmark.

I'm facing a real weird problem with EKF Localization. The filer gives me wrong error every time the robot is in parallel with a landmark. I've debugged the code many times but failed to solve the problem however I found out where is exactly the problem occurs. The following picture shows the scenario. The robot moves in a circular motion. There are four landmarks. I have indicted in the picture where the filer gives me wrong angle for the estimated state. As you see, when the robot is in parallel with all landmarks, I got a wrong angle for the estimated robot's pose.

This is another picture shows how the estimated angle is wrong where the red circle is the estimated robot's pose and the blue one is the actual robot's pose.

I did also track the problem numerically. What I found out is that the estimated measurement of landmark # 4 is in the opposite direction of the actual measurement of landmark # 4.

i = 1 <---- landmark 1 <200,0>

est_robot =
6.4545
21.1119
0.1246

Zobs =
194.9271
-0.2208
1.0000

Zpre =
194.6936
-0.2333
1.0000

real_robot =
6.2069
20.9946
0.1188

Mubar =
6.2844
21.7029
0.1201

i = 2 <---- landmark 2 <200,200>

est_robot =
6.2844
21.7029
0.1201

Zobs =
263.8102
0.5982
2.0000

Zpre =
263.2785
0.6239
2.0000

real_robot =
6.2069
20.9946
0.1188

est_robot =
6.2901
21.0100
0.0155

i = 3 <---- landmark 3 <-200,200>
est_robot =
6.2901
21.0100
0.0155

Zobs =
273.0734
2.2991
3.0000

Zpre =
273.1173
2.4114
3.0000

real_robot =
6.2069
20.9946
0.1188

est_robot =
6.2840
21.0462
0.0259

i = 4 <---- landmark 4 <-200,0>

est_robot =
6.2840
21.0462
0.0259

Zobs =
207.2696
3.1272 <--- the actual measurement of landmark 4
4.0000

Zpre =
207.3548
-3.0658  <--- this is the problem. (it should be 3.0658)
4.0000

real_robot =
6.2069
20.9946
0.1188

est_robot =
6.0210
20.8238
-0.5621


and this is how I computed the angles.

For the actual measurements,

Zobs = [          sqrt((map(i,1) - real_robot(1))^2 + (map(i,2) - real_robot(2))^2)        ;
atan2(map(i,2) - real_robot(2), map(i,1) - real_robot(1)) - real_robot(3);
i];

Zobs(1) = Zobs(1) + sigma_r*randn();
Zobs(2) = Zobs(2) + sigma_phi*randn();
Zobs(3) = i;
Zobs(2) = mod(Zobs(2), 2*pi);

if (Zobs(2) > pi) % was positive
Zobs(2) = Zobs(2) - 2*pi;
elseif (Zobs(2) <= -pi) % was negative
Zobs(2) = Zobs(2) + 2*pi;
end


For the predicted measurements

q    = (map(i,1) - est_robot(1))^2 + (map(i, 2) - est_robot(2))^2;
Zpre = [                                                             sqrt(q);
atan2(map(i,2) - est_robot(2), map(i,1) - est_robot(1)) - est_robot(3);
i];

if (Zpre(2) > pi) % was positive
Zpre(2) = Zpre(2) - 2*pi;
elseif (Zpre(2) <= -pi) % was negative
Zpre(2) = Zpre(2) + 2*pi;
end


It seems to me that you did not consider the fact that angles are not just real numbers but cyclic ($+ \pi = - \pi$).
No. The two angles $3.1272$ and $-3.0658$ are rather close and thus these numbers look perfectly fine. The angle difference is $0.09 \equiv 5.17^{\circ}$.
When computing the difference between two angles, you need to make sure the results lie in $[-\pi, +\pi]$ (e.g. as described here).
• ,I've wrapped the difference, so the angle falls in [pi,-p]. I've implemented the entire project in OpenGL and I'm facing same problem. I've wrapped the difference in [0,2pi] but I'm getting same problem. – CroCo Jul 31 '14 at 9:43