2
$\begingroup$

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.

enter image description here

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.

enter image description here

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 
$\endgroup$
1
$\begingroup$

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

$\endgroup$
3
  • $\begingroup$ ,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. $\endgroup$
    – CroCo
    Jul 31 '14 at 9:43
  • $\begingroup$ Well sorry, there is not much to say without more information. Again: Zpre and Zobs above seem fine, your bug is most likely in the code using Zpre and Zobs, which you did not paste. $\endgroup$
    – sebsch
    Jul 31 '14 at 21:31
  • $\begingroup$ I'm using an algorithm that is stated in Probabilistic Robotics book. $\endgroup$
    – CroCo
    Aug 1 '14 at 12:39

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.