Reference:
Simulataneous localization and mapping with the extended Kalman filter - `A very quick guide... with Matlab code!', Joan Sola, 5 Oct, 2014
状态量的表达
在EKF SLAM中的我们维持的状态量X是由机器人位姿R和路标点M组合而成。
用高斯系统表示的话,就是维护状态量的均值和方差。
均值 协方差
状态量初始化
系统伊始只有机器人状态没有观测,状态量x只包含机器人位姿R。
运动方程
假设有一个运动方程f,控制量u,扰动n。
把运动方程f作用到高斯系统上。
其中F是Jacobian矩阵,因为在EKF系统里面我们用的一阶泰勒展开去近似线性系统嘛;N是扰动n的协方差矩阵。
在EKF系统的状态量X中,只有机器人位姿R是跟时间相关(跟运动方程相关)的,路标位置是固定的。
这样的话雅可比矩阵就是:
EKF预测环节
如果我们避免中的繁琐运算,比如乘以1,乘以0,加0(这种运算什么情况才会有?),
则EKF中的预测部分可以写成
如果我们把协方差矩阵P当作三角矩阵存储(因为它是对称的),那就不需要计算,最终这组矩阵的复杂度为O(n)。
以下图来说,左边的向量表示的状态量,右边的矩阵是协方差矩阵。
更新的部分是机器人位姿R和,为图中深灰色部分,以及浅灰色的部分。
EKF更新环节
EKF中观测方程的形式为:
其中X是状态量,h为观测方程,v是噪声,y是带噪声的观测。
EKF中更新的形式为:
其中H是Jacobian矩阵而R是观测噪声的协方差矩阵。
其中是更新量的均值,是更新量的方差,K是Kalman Gain。
注意K分子分母上下两个P不是一样的维度,具体来说:
假如现在有一个机器人,2个路标点,P是7*7矩阵。
当一个观测进来之后,分子的P用表示,分母的P用表示。
是观测方程对所有状态量X的偏导,这里X是机器人pose和当前观测,所以是2*5的矩阵。
是所有状态X与机器人pose和该观测相关的部分,即是7*5的矩阵。
而只跟该观测有关,是5*5的矩阵。
最后K是7*2的,对所有7个状态量都有增益。
y是我们处理过的观测数据,比如路标是一个球,我们的维持的y是球心在图像平面的坐标,uv。
EKF中观测是一个一个处理的(应该有批量处理的办法?)
观测仅仅跟机器人状态R,传感器状态S和某个具体路标的状态有关。
对某个路标来说,有观测方程
它跟其他的路标点都无关,所以Jacobian也是稀疏的:
其中
因为有稀疏性,计算上面的和K的时候只要计算非零部分的值就好了。
以下图为例,左边灰色是更新部分,因为K影响整个系统,所以状态量和协方差矩阵整体都会更新。
但是实际上,对于某个路标,状态量中只有R和对应的路标更新,协方差矩阵中也是更新对应的。
那么这个点带来的EKF的更新部分就可以这样表示
因为最后P的更新,这组方程的复杂度是。
那对于所有k个观测到的路标,复杂度就是。
计算Z的逆的复杂度是O(1)。(跟EKF中cubic time 相反?)
K的复杂度是线性的O(n)。
地图的初始化
全观测
当一个新的路标被观测到时,如果观测到的信息可以还原路标的所有自由度的话,我们只需求观测方程h逆方程g技能结合R,S和观测y的信息还原出路标L的信息。
套到高斯系统的均值和方差方面,均值可以先算出来,方差要借用Jacobian:
然后把均值和方差加进状态量X和协方差矩阵P里面即可:
这组方程的复杂度是O(n)。
又以下面这个图表示,左边的状态量X末端增加了新的状态,右边的协方差矩阵P增加了新的协方差和交叉方差和
部分观测下的地标初始化
当我们的系统不是直接观测,观测方程L不可逆的时候。
就要把缺省的信息作为先验s加进来,这个先验有自己的均值和方差。
相似地,在EKF更新地时候先计算均值和Jacobian,然后更新协方差P:
最后把均值方差加入到系统中。
纯方位角观测的部分地标初始化
原文有点难懂,大概意思就是说,像相机这种sensor在距离的观测d上有不确定性,这个不确定性的范围(PCR)包括传感器的最小距离到无穷远。现在定义一个,就把PCR放到了。
现在有传感器在一开始的位置,观测到的方位,那么路标的位置就表示成:
这种情况下我们的路标点L可以表示为:
这样表示的好处是,当我们在机器人位置t有一个新的观测的时候,定义向量,有:
因为相机没办法得到距离,所以带入一个scale factor 把上式表达为:
这种情况下的直接观测方程就是:
相对的,我们知道机器人在初始状态下的状态和方位观测,则g方程可以表示为:
代码
%II.1 SIMULATOR
%a.motion
n = q .* randn(2,1); %perturbation vector
R = move(R, U, zeros(2,1));
%b. observations
for i=1:N
v = s.* randn(2,1) % measurement noise
Y(:,i)=observe(R,W(:,i))+v; %add measurement noise to observations
end
%II.2 ESTIMATOR
%a. create dynamic map pointers to be used hereafter
m = landmarks(landmarks~=0)'; %all pointers to landmarks --- 指向状态量X的第某个元素
%[0 0 0 0 4 0 6 0; 0 0 0 0 5 0 7 0]表示5号路标对应X的第4,5个元素。
rm = [r, m]; %all used states: robot and landmarks ---rm=[123,67]
% ( also OK is rm = find(mapspace); )
% b. Prediction ---robot motion
[x(r), R_r, R_n] = move(x(r),U,n); %r=[1 2 3] %plus perturbation on estimation
P(r,m)=R_r * P(r,m);%updates the covariance matrix
P(m,r)=P(r,m)';
P(r,r)=R_r*P(r,r)*R_r'+R_n*Q*R_n';
%c. Landmark correction --- known landmarks
lids = find(landmarks(1,:)); %returns all indices of existing landmarks
for i = lids
%expectation: gaussian {e,E}
l = landmarks(:,i)'; %landmark pointer
[e,E_r,E_l]=observe(x(r),x(l));%this is h(x) in EKF
rl = [r,l]; %pointers to robot and lmk.
E_rl = [E_r, E_l]; % expectation Jacobian
E = E_rl * P(rl, rl) * E_rl';
% the block E is consist of robot pose 3 dimension and landmarker 2
% dimension, So it is 5*5 dimens
%measurement of landmark i
Yi =Y(:,i);
%innovation: Gaussian {z,Z}
z = Yi - e; %this is z = y - h(x) in EKF
%we need values around zero for angles:
if z(2)>pi
z(2)=z(2)-2*pi;
end
if z(2)<-pi
z(2)=z(2)+2*pi;
end
Z=S+E;
%Individual compatibility check at Mahalanobis distance of 3-sigma
% (See appendix of documentation file 'SLAM course.pdf')
if z'*Z^-1*z<9
%kalman gain
K=P(rm,rl)*E_rl'*Z^-1;%this is K = P*H'*Z^-1 in EKF
%map update (use pointer rm)
x(rm)=x(rm)+K*z;
P(rm,rm)=P(rm,rm)-K*Z*K';
end
end
%d. Landmark Initialization --- one new landmark only at each iteration
lids = find(landmarks(1,:)==0); % all non-initialzed landmarks
if ~isempty(lids) %there are still landmarks to initialize
i = lids(randi(numel(lids))); %pick one landmark randomly, its index is i
l = find(mapspace==false,2) %pointer of the new landmark in the map
%[1 1 1 1 1 0 0 ... 0 0]mapsapce,逻辑数组,表示对应位置的状态量X是否启用。
% l 返回6和7
if ~isempty(l) %there is still sapce in the map
mapspace(l) = true; %block map space
landmarks(:,i)=l;%store landmark pointers
%measruement
Yi=Y(:,i);
%initialization
[x(l),L_r,L_y]=invObserve(x(r),Yi);
P(l,rm)=L_r*P(r,rm);%add P_lr P_rl P_ll to covariance matrix
P(rm,l)=P(l,rm)';
P(l,l)=L_r*P(r,r)*L_r'+L_y*S*L_y';
end
end
问题
有些路标的方差明现比其他点方差要大?
因为路标初始化的时候,用的是
P(l,l)=L_r*P(r,r)*L_r'+L_y*S*L_y';
其中L_r 是世界系路标点位置对robot pose的偏导。
L_r=[...
[1,0,-py*cos(a) - px*sin(a)]
[0,1,px*cos(a) - py*sin(a)]];
其中py px为路标在机器人系下的位置,a为机器人的航向角(世界系)。
可见L_r最后一列的大小跟py px相关,距离越远L_r越大,方差P(l,l)越大。
Reference:
Simulataneous localization and mapping with the extended Kalman filter - `A very quick guide... with Matlab code!', Joan Sola, 5 Oct, 2014