This tutorial is part of a series on Simultaneous Localization and Mapping (SLAM) using the extended Kalman filter. See the following for other tutorials in the series:

This tutorial builds on the previous two tutorials on localization. When doing SLAM with known measurement correspondence the number of landmarks is still fixed. Also, when we get a range and bearing measurement, we know to which landmark that measurement corresponds. This means we don’t need to use the maximum likelihood measurement association method from the previous tutorial. The big difference here is that we don’t have any prior information on where the landmarks are in the world. So instead of just using measurements to refine our pose estimate, we’ll also use them to estimate the landmark locations. Don’t worry though, the process is remarkably similar to simple localization. There’s just a couple of significant changes to highlight.

The State and Covariance Matrices

Since we don’t know where the landmarks are, we need simultaneously estimate their locations as well as our robot’s pose. This means the landmark locations are now part of the state of the system we’re trying to estimate. So the coordinates of the landmarks are now included in the state vector. When doing simple localization the state vector had dimension $3\times1$ . Now when doing SLAM the state vector has dimension $2n+3\times1$ where $n$ is the number of landmarks in our problem. In this formulation the coordinates of landmark $j$ ( $x_j$ and $y_j$ ) are found at indices $2i+2$ and $2i+3$ of the state vector.

Remember, our estimate of the system’s state is quantified by the state mean vector $\mu$ and covariance $\Sigma$ . So in order to estimate a larger $\mu$ vector we need to keep track of a larger $\Sigma$ matrix as well. Specifically, we’ll expand the covariance matrix from $3\times3$ to $2n+3\times2n+3$ where $n$ is again the number of landmarks.

The Update Step

To accommodate the larger size of our state vector we’ll need to make some changes to the EKF algorithm as well. Let’s start with the update step, where we update the pose of the robot based on the control inputs. We still need to update the elements of the state vector and covariance matrix that correspond to the robot’s pose. However, the robot’s control inputs clearly don’t affect the positions of the landmarks. So we need a way to update the robot’s pose without affecting the landmark estimates. The answer here is a special matrix we’ll call $F_x$ . $F_x$ is simply a $3\times2n+3$ matrix where the rightmost block is a $3\times3$ identity matrix and the other entries are zeros:

$\begin{equation*} F_k=\begin{bmatrix} 1 & 0 & 0 & 0 & \dots & 0 \\ 0 & 1 & 0 & 0 & \dots & 0 \\ 0 & 0 & 1 & 0 & \dots & 0 \end{bmatrix} \end{equation*}$

$F_x$ is used to map updates to the robot’s pose mean and covariance to the higher-dimensional space of the full state, which includes the landmark positions. So the update step now looks like this:

$\begin{align*} \bar{\mu}_k &= \mu_{k-1} + F_x^Tu_k' \\ G_k &= I_{2n+3}+F_x^Tg_kF_x \\ \bar{\Sigma}_k &= G_k \Sigma_{k-1} G_k^T + F_x^T R F_x \end{align*}$

where $u_k'$ is the motion update calculated from the control input at timestep $k$ (not the raw control input), $g_k$ is the motion Jacobian evaluated at timestep $k$ , $I_{2n+3}$ is a $2n+3\times2n+3$ identity matrix, and $R$ is the constant process noise covariance.

The Measurement Step

We need to make a similar change in the measurement step. A given measurement corresponds to only one landmark, so that measurement shouldn’t affect the estimates of the other landmarks. However, each measurement also affects the robot’s pose estimate. So we’ll need a way of mapping our corrections from the 2D landmark and 3D pose spaces to the full state space while ensuring only the relevant quantities in the mean vector, $\mu$ and covariance matrix, $\Sigma$ are affected. For this we need another $F$ matrix. This time it will be $F_{xj}$ , with the subscript $xj$ signifying that it relates the pose $x$ to landmark $j$ . $F_{xj}$ is a $5\times2n+3$ matrix. The $3\times3$ block in the upper right is an identity matrix, corresponding to the pose terms in the state and covariance matrix. $F_{xj}$ also has a $2\times2$ identity block occupying the lower two rows and starting at the index $2j+2$ where $j$ is the index of the landmark corresponding to the current measurement. All other entries in $F_{xj}$ are zeros:

$\begin{equation*} F_{xj} = \begin{bmatrix} 1 & 0 & 0 & \dots & 0 & 0 & \dots \\ 0 & 1 & 0 & \dots & 0 & 0 & \dots \\ 0 & 0 & 1 & \dots & 0 & 0 & \dots \\ 0 & 0 & 0 & \dots & 1 & 0 & \dots \\ 0 & 0 & 0 & \dots & 0 & 1 & \dots \end{bmatrix} \end{equation*}$

This cool matrix allows us to incorporate one measurement at a time, only updating the terms in the state and covariance matrices that correspond to the robot’s pose and the relevant landmark. We do this as follows:

$\begin{align*} H_k &= h_k F_{xj} \\ K &= \bar{\Sigma}H_k^T(H_k\bar{\Sigma}H_k^T+Q) ^{-1}\\ \mu_k &= \bar{\mu}_k + K(z-\hat{z}) \\ \Sigma_k &= (I_{2n+3} - KH_k)*\bar{\Sigma}_k \end{align*}$

where $h_k$ is the measurement Jacobian. With only these augmentations we can change the localization algorithm with known correspondence to a SLAM algorithm with known correspondence. Not too complicated, right?

The Complete Algorithm

The code for the complete algorithm is shown below. It is very similar to the code for EKF localization with known correlation with state vector and covariance matrices that now include landmarks and with the update and measurement steps augmented as discussed above. Note also that after the states are estimated, in lines 157 through 167, the system calculates the sum of squared errors for both the robot positions and the landmark positions. This gives us a rough way to judge the quality of our filter tuning. I’ll go into a more principled method for Kalman filter tuning in a later post, but this works well enough for demonstration purposes.

addpath ../common;

deltaT = .02;
alphas = [.1 .01 .18 .08 0 0]; % motion model noise parameters

% measurement model noise parameters
Q_t = [11.8 0;
       0    .18];
   
n_robots = 1;
robot_num = 1;
n_landmarks = 15;
[Barcodes, Landmark_Groundtruth, Robots] = loadMRCLAMdataSet(n_robots);
[Robots, timesteps] = sampleMRCLAMdataSet(Robots, deltaT);

% add pose estimate matrix to Robots
Robots{robot_num}.Est = zeros(size(Robots{robot_num}.G,1), 4);

start = 600;
t = Robots{robot_num}.G(start, 1);

% initialize state mean with the groundtruth
% pose at timestep zero
stateMean = [Robots{robot_num}.G(start,2);
            Robots{robot_num}.G(start,3);
            Robots{robot_num}.G(start,4)];
        
% reshapes stateMean to 1x(2*n_landmarks+3)
% and remaps motion updates to new state vector
F_x = [eye(3) zeros(3, 2 * n_landmarks)];

stateMean = F_x' * stateMean;

% initialize diagonal of pose covariance with small, nonzero values
% since we have a good estimate of the starting pose
stateCov = zeros(2 * n_landmarks + 3, 2 * n_landmarks + 3);
stateCov(1:3,1:3) = 0.001;

% initialize landmark variances to large values since we have
% no prior information on the locations of the landmarks
for i = 4:n_landmarks * 2 + 3
    stateCov(i,i) = 10;
end
   
measurementIndex = 1;

% set up map between barcodes and landmark IDs
% barcode values are NOT the same as landmark IDs
codeDict = containers.Map(Barcodes(:,2),Barcodes(:,1));

% loop through all odometry and measurement samples
% updating the robot's pose estimate with each step
% reference table 10.1 in Probabilistic Robotics
for i = start:size(Robots{robot_num}.G, 1)
    
    % update time
    t = Robots{robot_num}.G(i, 1);
    
    % get control inputs at current timestep
    u_t = Robots{robot_num}.O(i, 2:end);
    % update robot bearing to last timestep's estimate
    theta = stateMean(3, 1);
    % calculate rotation and translation over last timestep
    rot = deltaT * u_t(2);
    halfRot = rot / 2;
    trans = u_t(1) * deltaT;
    
    % calculate pose update in world frame
    poseUpdate = [trans * cos(theta + halfRot);
                  trans * sin(theta + halfRot);
                  rot];
              
    % updated state mean and constrain bearing
    % to +/- pi
    stateMeanBar = stateMean + F_x' * poseUpdate;
    stateMeanBar(3) = conBear(stateMeanBar(3));
    
    % calculate movement jacobian
    g_t = [0 0 trans * -sin(theta + halfRot);
           0 0 trans * cos(theta + halfRot);
           0 0 0];
    G_t = eye(2 * n_landmarks + 3) + F_x' * g_t * F_x;
     
    % calculate motion covariance in control space
    M_t = [(alphas(1) * abs(u_t(1)) + alphas(2) * abs(u_t(2)))^2 0;
           0 (alphas(3) * abs(u_t(1)) + alphas(4) * abs(u_t(2)))^2];
       
    % calculate Jacobian to transform motion covariance to state space
    V_t = [cos(theta + halfRot) -0.5 * sin(theta + halfRot);
           sin(theta + halfRot) 0.5 * cos(theta + halfRot);
           0 1];
    
    % update state covariance
    R_t = V_t * M_t * V_t';
    stateCovBar = (G_t * stateCov * G_t') + (F_x' * R_t * F_x);
    
    % get measurements for current timestep
    [z, measurementIndex] = getObservations(Robots, robot_num, t, measurementIndex, codeDict);

    % if measurements are available
    if z(3,1) > 1
        S = zeros(size(z,2),3,3);
        zHat = zeros(2, size(z,2));
        % loop over every measurement and use to correct state estimate
        for k = 1:size(z, 2) 
            j = z(3,k);
            
            % if the landmark has never been seen before
            % add it to the state vector
            if stateMeanBar(j*2 + 2) == 0
                landmark_pos = [z(1,k) * cos(z(2,k) + stateMeanBar(3));
                                z(1,k) * sin(z(2,k) + stateMeanBar(3))];
                stateMeanBar(2*j+2:2*j+3) = [stateMeanBar(1) + landmark_pos(1);
                                             stateMeanBar(2) + landmark_pos(2)];
                continue;
            end

            % compute predicted range and bearing to the landmark
            delta = [stateMeanBar(2*j+2) - stateMeanBar(1);
                     stateMeanBar(2*j+3) - stateMeanBar(2)];
            q = delta' * delta;
            r = sqrt(q); % predicted range to landmark
            
            % predicted bearing to landmark
            pred_bear = conBear(atan2(delta(2), delta(1)) - stateMeanBar(3));
            % create predicted measurement
            zHat(:,k) = [r;
                         pred_bear];
            % calculate measurement Jacobian    
            h_t = [-delta(1)/r -delta(2)/r  0   delta(1)/r delta(2)/r;
                   delta(2)/q    -delta(1)/q    -1  -delta(2)/q  delta(1)/q];
                     
            F_1 = [eye(3); zeros(2,3)];
            F_2 = [zeros(3,2); eye(2)];
            F_xj = [F_1 zeros(5,2*j-2) F_2 zeros(5,2*n_landmarks - 2*j)];
            
            H_t = h_t * F_xj;
            
            % compute Kalman gain
            K = stateCovBar * H_t' / (H_t * stateCovBar * H_t' + Q_t);
            
            % incorporate new measurement into state mean and covariance
            stateMeanBar = stateMeanBar + K * (z(1:2,k) - zHat(:,k));
            stateCovBar = (eye(2*n_landmarks+3) - (K * H_t)) * stateCovBar;
        end
    end
    
    % update state mean and covariance
    stateMean = stateMeanBar;
    stateMean(3) = conBear(stateMean(3));
    stateCov = stateCovBar;
    
    % add new pose mean to estimated poses
    Robots{robot_num}.Est(i,:) = [t stateMean(1) stateMean(2) stateMean(3)];
end

% calculate land_loss: sum of squares of error in final landmark position
land_loss = 0;
for i = 1:n_landmarks
    x_diff = stateMean(2 * i + 1) - Landmark_Groundtruth(i, 2);
    y_diff = stateMean(2 * i + 2) - Landmark_Groundtruth(i, 3);
    sq_err = x_diff^2 + y_diff^2;
    land_loss = land_loss + sq_err;
end
disp(land_loss);

p_loss = path_loss(Robots, robot_num, start);
disp(p_loss);
animateMRCLAMdataSet(Robots, Landmark_Groundtruth, timesteps, deltaT);

Bot Blog

building my robot army one sketch at a time

Intro to the EKF Step 3: SLAM With Known Correspondence

The State and Covariance Matrices

The Update Step

The Measurement Step

The Complete Algorithm