EKF-SLAM simulated in MATLAB

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=\begin{bmatrix} R\\M \end{bmatrix}=\begin{bmatrix} R\\ L_1\\ ...\\ L_n \end{bmatrix}

用高斯系统表示的话,就是维护状态量的均值和方差。

均值\bar{X}=\begin{bmatrix} \bar{R}\\ \bar{M} \end{bmatrix}=\begin{bmatrix} \bar{R}\\ \bar{L_1}\\ ...\\ \bar{L_n} \end{bmatrix}    协方差P=\begin{bmatrix} P_R_R & P_R_L\\ P_L_R & P_L_L \end{bmatrix}=\begin{bmatrix} P_R_R& P_R_L_1 &... &P_R_L_n \\ P_L_1_R&P_L_1_L_1 &... &P_L_1_L_n \\ ... &... &... &... \\ P_L_n_R & P_L_n_L_1 &... &P_L_n_L_n \end{bmatrix}

状态量初始化

系统伊始只有机器人状态没有观测,状态量x只包含机器人位姿R。

\bar{X}=\begin{bmatrix} x\\ y\\ \theta \end{bmatrix}=\begin{bmatrix} 0\\ 0\\ 0 \end{bmatrix} ,P=\begin{bmatrix} P_R_R \end{bmatrix}=\begin{bmatrix} 0 &0 &0 \\ 0 &0 &0 \\ 0& 0 &0 \end{bmatrix}

运动方程

假设有一个运动方程f,控制量u,扰动n。

X_{t+1}=f(X_{t},u,n)

把运动方程f作用到高斯系统上。

\bar{X}_{t+1}=f(\bar{X}_t,u,0) , P=F_XPF^{T}_X+F_nNF^{T}_n

其中F是Jacobian矩阵,因为在EKF系统里面我们用的一阶泰勒展开去近似线性系统嘛;N是扰动n的协方差矩阵。

F_X=\frac{\partial f(\bar{X},u)}{\partial X}, F_n=\frac{\partial f(\bar{X},u)}{\partial n}

在EKF系统的状态量X中,只有机器人位姿R是跟时间相关(跟运动方程相关)的,路标位置是固定的。

R_{t+1}=f_R(R_{t},u,n),M_{t+1}=M

这样的话雅可比矩阵就是:

F_X=\frac{\partial f(X)}{\partial X}=\frac{\partial f([R;M])}{\partial [R;M]}=\begin{bmatrix} \frac{\partial f_R}{\partial R} & 0 \\ 0 & I \end{bmatrix}

F_n =\frac{\partial f(X)}{\partial n}=\frac{\partial f([R;M])}{\partial n}=\begin{bmatrix} \frac{\partial f_R}{\partial n}\\0 \end{bmatrix}

EKF预测环节

如果我们避免P=F_XPF^{T}_X+F_nNF^{T}_n中的繁琐运算,比如乘以1,乘以0,加0(这种运算什么情况才会有?),

则EKF中的预测部分可以写成

\\\bar{R}\leftarrow f(\bar{R},u,0) \\P_{RR}\leftarrow \frac{\partial f_R}{\partial R}P_{RR}\frac{\partial f_R}{\partial R}^{T}+\frac{\partial f_R}{\partial n}N\frac{\partial f_R}{\partial n}^{T} \\P_{RM}\leftarrow \frac{\partial f_R}{\partial R}P_{RM} \\ \\P_{MR}\leftarrow P^{T}_{RM}

如果我们把协方差矩阵P当作三角矩阵存储(因为它是对称的),那就不需要计算\\P_{MR}\leftarrow P^{T}_{RM},最终这组矩阵的复杂度为O(n)。

以下图来说,左边的向量表示的状态量,右边的矩阵是协方差矩阵。

更新的部分是机器人位姿R和P_{RR},为图中深灰色部分,以及浅灰色的P_{RM},P_{MR}部分。

EKF更新环节

EKF中观测方程的形式为:

y=h(X)+v

其中X是状态量,h为观测方程,v是噪声,y是带噪声的观测。

EKF中更新的形式为:

\\K=\frac{PH^{T}_X}{H_XPH^{T}_X+R} \\\bar{X}\leftarrow \bar{X}+K(y-h(\bar{X})) \\P\leftarrow (I-KH_X)P

其中H是Jacobian矩阵H_X=\frac{\partial h(\bar{X})}{\partial X}而R是观测噪声的协方差矩阵。

其中y-h(\bar{X})是更新量的均值,H_XPH^{T}_X+R是更新量的方差,K是Kalman Gain。

注意K分子分母上下两个P不是一样的维度,具体来说:

假如现在有一个机器人,2个路标点,P是7*7矩阵。

当一个观测进来之后,分子的P用P_{up}表示,分母的P用P_{down}表示。

H_{X}是观测方程对所有状态量X的偏导,这里X是机器人pose和当前观测,所以H_X是2*5的矩阵。

P_{up}是所有状态X与机器人pose和该观测相关的部分,即P_{up}是7*5的矩阵。

P_{down}只跟该观测有关,是5*5的矩阵。

最后K是7*2的,对所有7个状态量都有增益。

y是我们处理过的观测数据,比如路标是一个球,我们的维持的y是球心在图像平面的坐标,uv。

EKF中观测是一个一个处理的(应该有批量处理的办法?)

观测仅仅跟机器人状态R,传感器状态S和某个具体路标L_i的状态有关。

对某个路标L_i来说,有观测方程

y_i=h_i(R,S,L_i)+v

它跟其他的路标点都无关,所以Jacobian也是稀疏的:

H_X=\begin{bmatrix} H_R &0 &... &0 &H_{L_i} &0 &... &0 \end{bmatrix}

其中\\H_R=\frac{\partial h_i(\bar{R},S,\bar{L_i})}{\partial R} \\H_{L_i}=\frac{\partial h_i(\bar{R},S,\bar{L_i})}{\partial L_i}

因为有稀疏性,计算上面的Z=H_XPH^{T}_X+R和K的时候只要计算非零部分的值就好了。

以下图为例,左边灰色是更新部分,因为K影响整个系统,所以状态量和协方差矩阵整体都会更新。

但是实际上,对于某个路标,状态量中只有R和对应的路标L_i更新,协方差矩阵中也是更新对应的P_{RR},P_{L_iL_i},P_{RL_i},P_{L_iR}

那么这个点带来的EKF的更新部分就可以这样表示

\\\bar{Z}=y_i-h_i(\bar{R},S,\bar{L_i}) \\Z=\begin{bmatrix} H_R&H_{L_i} \end{bmatrix}*\begin{bmatrix} P_{RR} & P_{RL_i}\\ P_{L_iR} & P_{L_iL_i} \end{bmatrix}*\begin{bmatrix} H^{T}_R \\H^{T}_{L_i} \end{bmatrix}+R \\K=\begin{bmatrix} P_{RR} & P_{RL_i}\\ P_{L_iR} & P_{L_iL_i} \end{bmatrix}*\begin{bmatrix} H^{T}_R \\H^{T}_{L_i} \end{bmatrix}*Z^{-1} \\\bar{X}\leftarrow \bar{X}+K\bar{Z} \\P\leftarrow P-KZK^T \\or \\P\leftarrow (I-KH)P

因为最后P的更新,这组方程的复杂度是O(n^{2})

那对于所有k个观测到的路标,复杂度就是O(kn^{2})

计算Z的逆的复杂度是O(1)。(跟EKF中cubic time O(n^{3})相反?)

K的复杂度是线性的O(n)。

地图的初始化

全观测

当一个新的路标被观测到时,如果观测到的信息可以还原路标的所有自由度的话,我们只需求观测方程h逆方程g技能结合R,S和观测y的信息还原出路标L的信息。

L_{n+1}=g(R,S,y_{n+1})

套到高斯系统的均值和方差方面,均值可以先算出来,方差要借用Jacobian:

\\\bar{L}_{n+1}=g(\bar{R},S,y_{n+1}) \\G_R=\frac{\partial g(\bar{R},S,y_{n+1})}{\partial R} \\G_{y_{n+1}}=\frac{\partial g(\bar{R},S,y_{n+1})}{\partial y_{n+1}}

\\P_{LL}=G_RP_{RR}G^T_R+G_{y_{n+1}}RG_{y_{n+1}}^T \\P_{LX}=G_RP_{RX}=G_R\begin{bmatrix} P_{RR} &P_{RM} \end{bmatrix}

然后把均值和方差加进状态量X和协方差矩阵P里面即可:

\\\bar{X}\leftarrow \begin{bmatrix} \bar{X}\\\bar{L}_{n+1} \end{bmatrix} \\P\leftarrow \begin{bmatrix} P &P^T_{LX} \\ P_{LX} & P_{LL} \end{bmatrix}

这组方程的复杂度是O(n)。

又以下面这个图表示,左边的状态量X末端增加了新的状态L_{n+1},右边的协方差矩阵P增加了新的协方差P_{L_n_{+1}L_n_{+1}}和交叉方差P^T_{LX}P_{LX}

部分观测下的地标初始化

当我们的系统不是直接观测,观测方程L不可逆的时候。

就要把缺省的信息作为先验s加进来,这个先验有自己的均值和方差。

L_{n+1}=g(R,S,y_{n+1},s)

相似地,在EKF更新地时候先计算均值和Jacobian,然后更新协方差P:

\\\bar{L}_{n+1}=g(\bar{R},S,y_{n+1},\bar{s}) \\G_R=\frac{\partial g(\bar{R},S,y_{n+1},\bar{s})}{\partial R} \\G_{y_{n+1}}=\frac{\partial g(\bar{R},S,y_{n+1},\bar{s})}{\partial y_{n+1}} \\G_s=\frac{\partial g(\bar{R},S,y_{n+1},\bar{s})}{\partial s}

\\P_{L_{n+1}L_{n+1}}=G_RP_{RR}G_R^T+G_{y_{n+1}}RG_{y_{n+1}}^T+G_{s}SG_{s}^T \\P_{LX}=G_RP_{RX}=G_R\begin{bmatrix} P_{RR} &P_{RM} \end{bmatrix}

最后把均值方差加入到系统中。

纯方位角观测的部分地标初始化

原文有点难懂,大概意思就是说,像相机这种sensor在距离的观测d上有不确定性,这个不确定性的范围(PCR)包括传感器的最小距离到无穷远。现在定义一个\rho =1/d_{min},就把PCR放到了[0,1/d_{min}]

现在有传感器在一开始的位置p_0,观测到的方位\alpha,那么路标的位置就表示成:

p=p_0+1/\rho \begin{bmatrix} cos\alpha \\ sin\alpha \end{bmatrix}

这种情况下我们的路标点L可以表示为:

L=\begin{bmatrix} \mathbf{P_0}\\ \alpha \\ \rho \end{bmatrix}\in \mathbb{R}^4

这样表示的好处是,当我们在机器人位置t有一个新的观测的时候,定义向量\mathbf{v=p-t},有:

\mathbf{v}\equiv \mathbf{p}-\mathbf{t}=(\mathbf{p}_0-\mathbf{t})+1/\rho \begin{bmatrix} cos\alpha \\ sin\alpha \end{bmatrix}

因为相机没办法得到距离,所以带入一个scale factor \rho把上式表达为:

\mathbf{v}\propto \rho (\mathbf{p}_0-\mathbf{t})+ \begin{bmatrix} cos\alpha \\ sin\alpha \end{bmatrix}

这种情况下的直接观测方程就是:

\mathbf{y}=h(\mathbf{R,L})=arctan(v_2/v_1)-\theta

相对的,我们知道机器人在初始状态下的状态R=\begin{bmatrix} \mathbf{t} & \theta \end{bmatrix}^T和方位观测\mathbf{y}=\phi,则g方程可以表示为:

\mathbf{L}=g(\boldsymbol{R},\boldsymbol{y},\rho )=\begin{bmatrix} \boldsymbol{t}\\ \theta+\phi \\ \rho \end{bmatrix}

代码

    %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

©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页