EKF-SLAM simulated in MATLAB

最新推荐文章于 2024-03-07 19:06:47 发布

NoEdUl

最新推荐文章于 2024-03-07 19:06:47 发布

阅读量338

点赞数

分类专栏：算法基础文章标签：算法人工智能卡尔曼滤波算法 slam matlab

本文链接：https://blog.csdn.net/u012947309/article/details/107680442

版权

算法基础专栏收录该内容

1 篇文章 0 订阅

订阅专栏

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

NoEdUl

关注

0
点赞
踩
5

收藏

觉得还不错? 一键收藏
0
评论
EKF-SLAM simulated in MATLAB

状态量的表达在EKF SLAM中的我们维持的状态量X是由机器人位姿R和路标点M组合而成。用高斯系统表示的话，就是维护状态量的均值和方差。均值协方差状态量初始化系统伊始只有机器人状态没有观测，状态量x只包含机器人位姿R。运动方程假设有一个运动方程f，控制量u，扰动n。把运动方程f作用到高斯系统上。其中F是Jacobian矩阵，因为在EKF系统里面我们用的一阶泰勒展开去近似线性系统嘛;N是扰动n的协方差矩阵。在EKF系统的状态量X中，只有机器人...
复制链接

扫一扫