EKF SLAM学习笔记

InfoRich 2022-04-20

展开全文

EKF

ekf是扩展卡尔曼滤波的缩写：Extended Kalman Filter

本文将从概率论中的相关概念说起，逐步讲解到贝叶斯滤波、卡尔曼滤波、和扩展卡尔曼滤波。重点将放在两个例子上：ekf定位和ekf slam的python程序。（不涉及卡尔曼增益的推导）

卡尔曼滤波

概率论基础

关于随机变量、概率密度函数、正态分布、贝叶斯法则等建议首先了解基本概念。

随机变量的期望：

连续形式：

import numpy as npimport randomx=[3,1,2]p=[0.1,0.3,0.4]E_x=np.sum(np.multiply(x,p))print(E_x)

1.4

随机变量的期望可以看做代表了随机变量的平均值，而随机变量的方差描述了随机变量围绕平均值的分散情况：

x=np.random.randn(10)np.var(x)

0.94945573315067155

多元分布指的是具有多个变量的随机分布。例如二维空间中的机器人位置需要用x和y两个变量描述，为了描述它们，需要用在x和y上都有均值的二元正态分布。

对于多元分布来说，均值 μ \muμ 可以用向量来表示：

同样地，变量的方差也可以矩阵来表示，只不过，在这里需要引入协方差的概念：就像每个变量的值都有其方差一样，也可能存在表征它们“一起变化”的情况的值。这也是衡量两个数据变量如何相互关联或“相关”的一种度量。

例如，随着身高增加，体重通常也增加。这些变量是相关的。它们是正相关的，因为随着一个变量变大，另一个变量也大概率会变大。

我们可以用一个协方差矩阵来表示多元正态分布的协方差：

对角 - 各个变量的自身方差

非对角 - 第ith个变量和第jth变量的协方差

x_cor=np.random.rand(1,10)y_cor=np.random.rand(1,10)np.cov(x_cor,y_cor)

array([[ 0.08426048, -0.0228549 ],[-0.0228549 , 0.05913773]])

np.cov(x_cor,y_cor,bias=1)

array([[ 0.07583444, -0.02056941],[-0.02056941, 0.05322396]])

这里简要说一下样本和总体的区别，如果说能够确定一批数据拢共就这么多了，那么计算协方差就是当做总体来计算，而如果真实数据是无穷无尽或极大数量的，得到的数据只是从中采样得到的，那就是真实数据的样本，需要当做样本来计算协方差。

高斯分布

根据“中心极限定理”，随着样本数量的增加，随机独立变量的n个样本的平均值趋于服从正态（高斯）分布。（一般n>=30)

import matplotlib.mlab as mlabimport matplotlib.pyplot as pltimport mathimport scipy.stats

mu = 0variance = 5sigma = math.sqrt(variance)x = np.linspace(mu - 5*sigma, mu + 5*sigma, 100)plt.plot(x,scipy.stats.norm.pdf(x, mu, sigma))plt.show()

多元高斯分布（以二维为例）：注意协方差矩阵的主对角线就是方差，非对角线上的就是两个变量间的协方差。就下面的二元高斯分布而言，协方差越大，图像越扁，也就是说两个维度之间越有联系。

#Example from:#https://scipython.com/blog/visualizing-the-bivariate-gaussian-distribution/import numpy as npimport matplotlib.pyplot as pltfrom matplotlib import cmfrom mpl_toolkits.mplot3d import Axes3D
# Our 2-dimensional distribution will be over variables X and YN = 60X = np.linspace(-3, 3, N)Y = np.linspace(-3, 4, N)X, Y = np.meshgrid(X, Y)
# Mean vector and covariance matrixmu = np.array([0., 1.])Sigma = np.array([[ 1.9 , -0.5], [-0.5,  0.5]])
# Pack X and Y into a single 3-dimensional arraypos = np.empty(X.shape + (2,))pos[:, :, 0] = Xpos[:, :, 1] = Y
def multivariate_gaussian(pos, mu, Sigma):    '''Return the multivariate Gaussian distribution on array pos.
    pos is an array constructed by packing the meshed arrays of variables    x_1, x_2, x_3, ..., x_k into its _last_ dimension.
    '''
    n = mu.shape[0]    Sigma_det = np.linalg.det(Sigma)    Sigma_inv = np.linalg.inv(Sigma)    N = np.sqrt((2*np.pi)**n * Sigma_det)    # This einsum call calculates (x-mu)T.Sigma-1.(x-mu) in a vectorized    # way across all the input variables.    fac = np.einsum('...k,kl,...l->...', pos-mu, Sigma_inv, pos-mu)
    return np.exp(-fac / 2) / N
# The distribution on the variables X, Y packed into pos.Z = multivariate_gaussian(pos, mu, Sigma)
# Create a surface plot and projected filled contour plot under it.fig = plt.figure()ax = fig.gca(projection='3d')ax.plot_surface(X, Y, Z, rstride=3, cstride=3, linewidth=1, antialiased=True,                cmap=cm.viridis)
cset = ax.contourf(X, Y, Z, zdir='z', offset=-0.15, cmap=cm.viridis)
# Adjust the limits, ticks and view angleax.set_zlim(-0.15,0.2)ax.set_zticks(np.linspace(0,0.2,5))ax.view_init(27, -21)
plt.show()