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
运行示意图就是这样