Void’s Vault

Knowledge source for efficiency.

Extended Kalman Filter With Localization and Known Correspondences

Still working hard on Probabilistic Robotics. Here’s the implementation of the EKF algorithm that enables localization with known correspondences.

First, the test script: a robot that turn around (3,3). He localize itself by measuring the position of two features, one at (3,3) and the other at (0,0).

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
clearvars;
clf;
N = 17; % Number of iterations
v = 3*pi/10;
w = pi/10;
u = [v;w];
dt = 1;
X1 = [3;0;0]; %Starting pose
mu1 = X1;
SigmaX = 0.01;
SigmaY = 0.01;
SigmaT = 0.001; %Error over theta
sigma1 = [SigmaX 0 0; 0 SigmaY 0; 0 0 SigmaT]; %Error on the pose
SigmaR = 0.001;  %Range
SigmaP = 0.001;  %Bearing
SigmaS = 0.0001; %Signature
sigmaZ = [SigmaR; SigmaP; SigmaS]; %Error on the measurements
m=[0 3; 0 3; 1 2];  %map with 2 features
c = [1,2]; %Do not change for the current problem.

MuA = zeros(N+1, numel(mu1)); %filtered pose
GT = zeros(N+1, numel(X1)); %ground truth
MuA(1,:) = mu1;
GT(1,:) = X1;

for iter=2:1:N+1
    X0 = X1;
    X0(3) = mod(X0(3) + pi, 2*pi)-pi;
    mu0 = mu1;
    sigma0 = sigma1;

    X1 = X0 + [((-v/w)*sin(X0(3))+(v/w)*sin(X0(3)+w*dt) + randn*SigmaX);
                ((v/w)*cos(X0(3))-(v/w)*cos(X0(3)+w*dt) + randn*SigmaY);
                (w*dt + randn*SigmaT)];

    X1(3) = mod(X1(3) + pi, 2*pi)-pi;
    GT(iter,:) = X1;
    q = [((m(1,1) - X1(1)).^2 + (m(2,1) - X1(2)).^2);
            ((m(1,2) - X1(1)).^2 + (m(2,2) - X1(2)).^2)];

     z1(:,1) = [(sqrt(q(1)) + randn*SigmaR);
               mod((atan2(m(2,1) - X1(2),m(1,1) - X1(1))-X1(3) + randn*SigmaP + pi),2*pi)-pi;
               (m(3,1) + randn*SigmaS)];
    z1(:,2) = [(sqrt(q(2)) + randn*SigmaR);
               mod((atan2(m(2,2) - X1(2),m(1,2) - X1(1))-X1(3) + randn*SigmaP + pi),2*pi)-pi;
               (m(3,2) + randn*SigmaS)];

    [mu1, sigma1, pz1] = EKF_localization_known_correspondences(mu0, sigma0, u, z1, sigmaZ, c, m);
    MuA(iter,:) = mu1;
end

h(1) = plot(GT(:,1),GT(:,2),'go');
hold on;
h(2) = plot(MuA(:,1),MuA(:,2),'b+');
legend(h,{'Ground Truth' 'UKF Estimate'}, 'Location', 'SouthEast');

Pffew! Now, let’s see the EKF. Enjoy!

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
function [ mu1, sigma1 ] = EKF_localization_known_correspondences( mu0, sigma0, u1, z1, sigmaZ, c1, m)
% The EKF localisation algorithm, for a feature-based map and a robot
% equipped with sensors for measuring range and bearing. This version
% assumes knowledge of the exact correspondences
dt = 1;
a1 = 0.1;
a2 = 0.1;
a3 = 0.1;
a4 = 0.1;
theta = mu0(3);
v = u1(1);
w = u1(2);

%Prediction step
G1 = [1 0 (-(v/w)*cos(theta)+(v/w)*cos(theta+w*dt));
      0 1 (-(v/w)*sin(theta)+(v/w)*sin(theta+w*dt));
      0 0 1];

V1 = [(-sin(theta)+sin(theta+w*dt))/w ((v*(sin(theta)-sin(theta+w*dt))/(w.^2))+(v*cos(theta+w*dt)*dt/w));
      (cos(theta)-cos(theta+w*dt))/w ((-v*(cos(theta)-cos(theta+w*dt))/(w.^2))+(v*sin(theta+w*dt)*dt/w));
      0 dt];

M1 = [(a1*(v.^2)+a2*(w.^2)) 0;
        0 (a3*(v.^2)+a4*(w.^2))];

mu1_ = mu0 + [(-v/w)*sin(theta)+(v/w)*sin(theta+w*dt);
               (v/w)*cos(theta)-(v/w)*cos(theta+w*dt);
               w*dt];

sigma1_ = G1*sigma0*G1' + V1*M1*V1';

%Correction step
Q1 = [sigmaZ(1) 0 0;
      0 sigmaZ(2) 0;
      0 0 sigmaZ(3)];

%For all observed features
z1_ = zeros(3,numel(c1));
S1 = zeros(3,3,numel(c1));

for i=1:1:numel(c1)
    j = c1(i);
    q = (m(1,j) - mu1_(1)).^2 + (m(2,j) - mu1_(2)).^2;
    z1_(:,i) = [sqrt(q);
           mod(atan2(m(2,j) - mu1_(2),m(1,j) - mu1_(1)) - mu1_(3) + pi,2*pi)-pi;
           m(3,j)]; %modulo allows us to keep angle between [-pi,pi]    

    H1 = [(m(1,j)-mu1_(1))/(-sqrt(q)) (m(2,j)-mu1_(2))/(-sqrt(q)) 0;
          (m(2,j)-mu1_(2))/q          (m(1,j)-mu1_(1))/q         -1;
          0 0 0];

    S1(:,:,i) = H1*sigma1_*H1' + Q1;
    K1 = sigma1_*H1'*inv(S1(:,:,i));
    mu1_ = mu1_ + K1*(z1(:,i) - z1_(:,i));
    mu1_(3) = mod(mu1_(3) + pi, 2*pi)-pi;
    sigma1_ = (eye(3)-K1*H1) * sigma1_;
end
mu1 = mu1_;
sigma1 = sigma1_;
end

Note that the algorithm also includes the evaluation of a likelihood for the value of mu1 found by the filter. Because I have not figured out that part yet, I did not put this unessential piece of code in this article. Maybe later!