EKF Localization when robot is in parallel with a landmark.

Question

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];


     % add Gaussian noise 
     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 

Answer 1

It seems to me that you did not consider the fact that angles are not just real numbers but cyclic ($+ \pi = - \pi$).

What I found out is that the estimated measurement of landmark # 4 is in the opposite direction of the actual measurement of landmark # 4.

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).