《概率机器人》学习总结
《Probabilistic Robotics》中文译名为《概率机器人》
感谢我的导师在研究生阶段开设了这门课程，带领我们简单的过了一下这本书。
相关资料：http://ais.informatik.uni-freiburg.de/teaching/ss16/robotics/
最后老师留了完成EKF Localization或者PF Localization的代码片段的题目。具体的模型是里程计运动模型和基于地标的测量模型。
需要完成的编程是滤波器操作的代码。采用EKF的话可以借鉴《概率机器人》书中的第二部分里面的EKF的详细的伪代码。采用PF的话由于没有详细的伪代码，可以借鉴书上FAST SLAM部分整理一下思路。
接下来是代码的实现：
function [mu, sigma, predMu, predSigma, zHat, pOfZ, G, V, H, K] = ...ekfUpdate( mu, sigma, ...u, filterAlphas, ...z, filterBeta, ...markerId, FIELDINFO)
% EKF Update step
%
% INPUT:
% mu - state mean at time t-1
% sigma - state covariance at time t-1
% u - control at time t
% filterAlphas - motion model noise
% z - observation at time t
% filterBeta - observation noise variance
% markerId - ID of the marker we see at time t (get observation from)
% FIELDINFO - Field details for observation function
%
% OUTPUTS:
% mu - state mean at time t
% sigma - state covar at time t
% predMu - state mean from EKF prediction step
% predSigma - state covar from EKF prediction step
% zHat - expected observation
% pOfZ - observation likelihood
% G - Jacobian of dynamics w.r.t state_t-1
% V - Jacobian of dynamics w.r.t control_t
% H - Observation jacobian
% K - Kalman gain
% Init vars
Delta_rot1 = u(1);
Delta_trans = u(2);
Delta_rot2 = u(3);
Pos_prev_x = mu(1);
Pos_prev_y = mu(2);
Pos_prev_theta = mu(3);% --------------------------------------------
% Prediction step
% --------------------------------------------%prediction_mu:
predMu = prediction(mu,u);%Jacobians:
%G_t:
G=eye(3,3);
G(1,3) = -Delta_trans*sin(Pos_prev_theta + Delta_rot1);
G(2,3) = Delta_trans*cos(Pos_prev_theta + Delta_rot1);
G_trans = G.';
%V_t:
V=zeros(3,3);
V(1,1) = -Delta_trans*sin(Pos_prev_theta + Delta_rot1);
V(1,2) = cos(Pos_prev_theta + Delta_rot1);
V(2,1) = Delta_trans*cos(Pos_prev_theta + Delta_rot1);
V(2,2) = sin(Pos_prev_theta + Delta_rot1);
V(3,1) = 1;
V(3,3) = 1;
V_trans = V.';
%motion noise:
M_t = noiseFromMotion(u,filterAlphas);
M = [M_t(1),0,0;0,M_t(2),0;0,0,M_t(3)];
%prediction co-variance:
predSigma = G*sigma*G_trans + V*M*V_trans;%--------------------------------------------------------------
% Correction step
%--------------------------------------------------------------% Compute expected observation and Jacobian
%expected observation:
zHat = observation(predMu,FIELDINFO,markerId);
%Observation Jacobian:
%use equation P134
H = zeros(2,3);
%H = zeros(1,3);
dx = FIELDINFO.MARKER_X_POS( int32(markerId)) - Pos_prev_x;
dy = FIELDINFO.MARKER_Y_POS( int32(markerId)) - Pos_prev_y;
ds = dy/dx;
%dr = sqrt( dx^2 + dy^2);
%H(row_H(1),col_H(1)) = (-dx)/dr;
%H(row_H(1),col_H(2)) = (-dy)/dr;
H(1,1) = dy/((1+ds^2)*dx^2);
H(1,2) = -1/((1+ds^2)*dx);
H(1,3) = -1;%predicted observation co-variance:
H_trans = H.';
Q = [filterBeta,0;0,0.0001];
%Q = filterBeta;
S = H*predSigma*H_trans + Q;
% Innovation / residual covariance% Residual% Likelihood% Kalman gain
K = predSigma*H_trans/S;% Correction
zDelta = z - zHat;
%innovation = zDelta + H*(mu - predMu);
zDelta(1) = minimizedAngle(zDelta(1));
pOfZ = likelihood(S(1,1),zDelta(1));
%pOfZ = likelihood(S,innovation);
mu = predMu + K*zDelta;
mu(3) = minimizedAngle(mu(3));
I = eye(3,3);
sigma = (I - K*H)*predSigma;
end
这里假设观测模型仅返回路标信息，和机器人的朝向信息，没有返回距离信息。采用的模型具体可以参考中文的书上P134页地标的测量。
这里需要注意的是返回的角度值，需要控制在(-pi,pi]之间，包括机器人位姿包含的角度值也需要控制在这之间。
额外需要注意的是噪声的处理。噪声这里假设传感器观测路标时是没有噪声的，仅在方向角度上有噪声。这就导致求雅可比矩阵时出现了不满秩的情况。而MATLAB在计算时这样会出错，所以在本代码中赋了一个很小的数。这个在计算测量似然时有很大的影响。本文给出的代码是在计算测量似然时直接进行降维处理，在计算卡尔曼增益时并未降维。
本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！