EKF

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

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

1 卡尔曼滤波

概率论基础

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

随机变量的期望：

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

1.4

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

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

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

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

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

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

高斯分布

import matplotlib.mlab as mlab
import matplotlib.pyplot as plt
import math
import scipy.stats

mu = 0
variance = 5
sigma = 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 np
import matplotlib.pyplot as plt
from matplotlib import cm
from mpl_toolkits.mplot3d import Axes3D

# Our 2-dimensional distribution will be over variables X and Y
N = 60
X = np.linspace(-3, 3, N)
Y = np.linspace(-3, 4, N)
X, Y = np.meshgrid(X, Y)

# Mean vector and covariance matrix
mu = np.array([0., 1.])
Sigma = np.array([[ 1.9 , -0.5], [-0.5,  0.5]])

# Pack X and Y into a single 3-dimensional array
pos = np.empty(X.shape + (2,))
pos[:, :, 0] = X
pos[:, :, 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 angle
ax.set_zlim(-0.15,0.2)
ax.set_zticks(np.linspace(0,0.2,5))
ax.view_init(27, -21)

plt.show()

二维

贝叶斯滤波

首先，我们为什么要提到高斯分布等以上概率论内容：对于一个最基本的机器人定位问题来讲，如果机器人的运动模型是准确无误的，且机器人自身的里程计、imu等内部传感器是准确的、没有误差的，那么我们就已经能够准确定位机器人了——速度1m/s的机器人在1s后肯定会出现在1m距离以外；另一方面来说，如果机器人的GPS、激光雷达等传感器是准确的、没有误差的，那么我们也能通过机器人在地球坐标系中的位置或机器人在环境中的相对位置来准确定位机器人。

可惜的是，对于现实中的机器人来讲，我们很难得到准确的机器人运动模型，从机器人的里程计、imu、GPS、激光雷达等传感器中读到的信号也充满噪声，所以机器人的定位才不是一个简单的问题。

我们可以将机器人的状态（包括但不限于位置、角度、速度等）用多维随机变量来表示，一般取每一维变量的期望（也就是平均值）作为该时刻下该状态的估计值，用方差来表示该状态的噪声水平（模型误差、传感器噪声等）。

下面让我们跳过基于贝叶斯法则的一系列推导，直接快进到贝叶斯滤波：

The basic filter algorithm is:

卡尔曼滤波器是高斯噪声下线性系统状态的最佳观测器，卡尔曼增益保证了状态向量均方误差最小。

参考链接：

卡尔曼滤波 – 从推导到应用(一)

PythonRobotics/Localization