Automotive Vision_6 SLAM

EKF-SLAM

• 增量计算
  更新在机器人位置和标志的知识增量的通过每一个新的观测
• 以一个卡尔曼滤波的形式展示
  系统状态：标记点的先验知识，机器人的姿势
  状态传递：机器人从一个点到两一个点
  观察：相关的标记点位置到么一个机器人位置
• 未知的变量
  机器人位置和方向
  标记的位置
• 状态空间
• 
• 状态传递
• 
• 观察点
• 
• 用EKF去估计
• 

SLAM matlab代码

load('data_lecture.mat');
%有一个初始位置
state = initial_state;
cov = 1e-100 * eye(3);
measurement_radius = 2;
stepwise = false;

% state and covariance history
states = [];
states(1).state = state;
states(1).cov = cov;
% prediction-innovation-loop
for t_k = 1:size(odom_meas,2)-1
    [state_pred, cov_pred] = slam_prediction (state, cov, odom_meas(:,t_k), noise_odom);
    observations = get_landmark_measurements(true_poses(:,t_k+1), true_landmarks, measurement_radius, noise_observation);
    
    if (isempty(observations))
        states(t_k + 1).state = state_pred;
        states(t_k + 1).cov = cov_pred;
        state = state_pred;
        cov = cov_pred;
    else
        [state_est, cov_est] = slam_innovation (state_pred, cov_pred, observations, noise_observation);
        states(t_k + 1).state = state_est; %#ok<*SAGROW>
        states(t_k + 1).cov = cov_est;
        state = state_est;
        cov = cov_est;
    end
    states(t_k + 1).pstate = state_pred;
    states(t_k + 1).pcov = cov_pred;
    states(t_k + 1).observations = observations;
    
    slam_visualize (states, observations, [], [-8 8 -8 8]);
    if (stepwise)
        ginput;
    else
        pause (0.2);
    end
end

SLAM Predict

function [ state_pred, cov_pred] = slam_prediction(state, cov, odom, noise_odom)
%SLAM_PREDICTION The prediction step of the extended kalman filter
% Input:
%  state: the current state 
%  cov: the current covariance matrix
%  odom: The delta motion of the agent
%  noise_odom: The covariance matrix for the delta motion
%
% Output: 
%  state_pred: The state after the prediction
%  cov_pred: The covariance after the prediction

cosphi = cos(state(3));
sinphi = sin(state(3));

state_pred = state;
cov_pred = cov;

A = [cosphi -sinphi 0; sinphi cosphi 0; 0 0 1];
state_pred(1:3) = state(1:3) + A * odom;

F = [1 0 -sinphi*odom(1)-cosphi*odom(2); 0 1 cosphi*odom(1)-sinphi*odom(2); 0 0 1];
cov_pred(1:3,1:3) = F * cov(1:3,1:3) * F' + noise_odom;


end

SLAM get_landmark_measurement

function measurements = get_landmark_measurements(true_pose, landmarks, radius, noise_measurement)
    num_meas = 0;
    measurements = [];
    phi = true_pose(3);
    for i = 1:size(landmarks,2)
        delta = landmarks(:,i) - true_pose(1:2);
        R = [ cos(phi) sin(phi); ...
             -sin(phi) cos(phi)];
        % check for distance
        if (delta'*delta <= radius*radius)%他这里还要判断一下landmark是不是太远
            num_meas = num_meas + 1;
            measurements(num_meas).id = i; %#ok<*AGROW>
            measurements(num_meas).pos = R*delta + normrnd(0,sqrt(noise_measurement),2,1);
        end
    end
end

SLAM Innovation

function [ state_est, cov_est] = slam_innovation(state, cov, observations, noise_observation)
% SLAM_INNOVATION The innovation of the EKF-SLAM
% Input
%  state: The current (predicted) state
%  cov: The current (predicted) covariance matrix
%  observations: The list of observations as [ID_1, x_1, y_1; ... ID_N,
%                x_N, y_N].
%  noise_observation: The variance of the measurement (a scalar).
%
% Output
%  state_est: The estimated state
%  cov_est: The estimated covariance

num_landmarks = (length(state)-3)/2;

%%% Initialization code

if (length(observations) == 0 )
    return;
end

H = zeros(2*length(observations), length(state));
observation_vec = zeros(length(observations)*2,1);
obs_model = zeros(length(observations)*2,1);

cosphi = cos(state(3));
sinphi = sin(state(3));

for i=1:length(observations)
    id = observations(i).id;
    if (id > num_landmarks)
        % extend the state dynamically       
        no_ext_state = (id-num_landmarks)*2;
        state = [state; zeros(no_ext_state,1)];                               
        cov = [cov, zeros(3+2*num_landmarks, no_ext_state); zeros(no_ext_state, 3+2*num_landmarks), 1e10*eye(no_ext_state)];                        
        H = [H zeros(size(H,1), no_ext_state)];
        
        num_landmarks = (length(state)-3)/2;
    end
        
    %%% Implement your code here (2. Jacobian of observation matrix H)    
    d = state(2+2*id:3+2*id)-state(1:2);    
    H((i*2)-1, 1:3) = [-cosphi -sinphi -sinphi*d(1)+cosphi*d(2)];
    H((i*2), 1:3) = [sinphi -cosphi -cosphi*d(1)-sinphi*d(2)]; 
    H((i*2)-1, 2+id*2:3+id*2) = [cosphi sinphi];
    H((i*2), 2+id*2:3+id*2) = [-sinphi cosphi];
    
    observation_vec(i*2-1:i*2) = observations(i).pos;
    obs_model(i*2-1:i*2) = [cosphi*d(1)+sinphi*d(2); -sinphi*d(1)+cosphi*d(2) ];
end

num_landmarks = (length(state)-3)/2;

%%% Implement your code here (3. Innovation of EKF)
K = cov*H'*(H*cov*H'+noise_observation*eye(2*length(observations)))^-1;
state_est = state+K*(observation_vec-obs_model);
cov_est = (eye(3+2*num_landmarks)-K*H)*cov;
end

运行示意图就是这样