机器人-数学工具

1. 基本概念

• LIDAR（LIght Detection And Ranging）：光探测和测距
• LADAR（LAser Detection And Ranging）：激光探测和测距
• $均值、方差、标准差$：用于描述一维随机变量 (即一维随机变量$X=(x)$，多个样本)
• $协方差、相关系数、协方差矩阵$：用于描述多维随机变量（即多维随机变量$X=(x,y,z)$, 多个样本）
• $协方差矩阵$：它计算的是不同维度之间的协方差，而不是不同样本之间
• 协方差矩阵定义了我们数据的传播（方差）和方向（协方差）
• 协方差<->特征向量<->雅可比矩阵

1.1 概率基本概念

1.1.1 随机变量

• 不确定性（Uncertainty）：不确定性在现实世界中是客观存在的；在现实世界中，不确定性来自于片面的、间接的和模糊的观察，观测值（e.g. sensor noise），动作执行（有限制的控制系统）
• 概率论（Probability Theory）：$是处理不确定性问题最强有力的方式$
• 随机变量（Random Variable）：用于表示一个不确定性的量，记作：$x$
  • 连续随机变量（Continuous）：其值为实数值（有限或无限的），其概率分布$f(x)$叫做概率密度函数（pdf: Probability Density Function）。其特性为：$$p(x)\ge 0\int f(x)dx=1$$
    
  • 离散随机变量（Discrete）：其值为预定义的集合(有序、无序、有限或无限的)，其概率分布$p(x)$叫做概率质量函数（Probability Mass Function）,经常以直方图或Hinton图表示，其特性为：
    $$p(x)\ge 0\sum p(x)=1$$
    
  • 正态分布概率密度函数
    $$X\sim N(\mu ,{\sigma }^2)f(x)=\frac{1}{\sqrt{2\pi }\sigma }{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}$$

1.1.2 随机向量

• 随机向量（Random Vector）：包含多个随机变量的向量为随机向量

• 正态分布：
  $$X=[x_1,x_2,...,x_k{]}^T：k个变量,X\sim N_k(\mu ,\Sigma )$$

• 均值向量：
  $$\mu =E[X]=[E[x_1],E[x_2],...,E[x_k]{]}^T$$

• kxk阶协方差矩阵:
  $$\Sigma =:E[(X-\mu )(X-\mu {)}^T]=[Cov[X_i,Y_j];1\le i,j\le k]$$

• 概率密度函数：
  $$f_X(x_1,...,x_k)=\frac{1}{\sqrt{(2\pi {)}^k|\Sigma |}}{e}^{-\frac{1}{2}(X-\mu {)}^T{\Sigma }^{-1}(X-\mu )}$$

1.1.3 概率

• 概率（Probability）：指随机变量取某一个值的机率，记作：$p(x)$

• 联合概率（Joint Probability）：指两个或多个事件同时发生的概率，记作：$p(x,y,z)$，其随机变量可能全部是离散的，或全部是连续的，或者是混合的；其总和或积分值一定是1。其中$p(x,y,z)可以记作p(X),X=[x,y,z{]}^T$

• 边缘概率（Marginal Probability）：指一个事件$x$发生的概率，记作：$p(x)$，根据 多个随机变量的联合概率求部分随机变量的概率的过程（求和或积分），被称为边缘化（marginalization）
  $$f(x)=\int f(x,y)dyp(y)=\int f(x,y)dx(x,y是连续的)$$
  $$p(x,y)=\sum _w\int f(x,y,z,w)dz(x,y是离散的，z是连续的，w是离散的)$$

• 条件概率（Conditional Probability）：对于联合概率$p(x,y）$，当$y=y^{\ast }$时$x$的概率，记作：$p(x|y=y^{\ast })$，简记为$p(x|y)$。$p(x,y)$的积分或求和的结果为1，而$p(x|y=y^{\ast })$只是其中的一部分，所以其积分或和小于1，为满足其积分或和的结果为1，需要对其进行归一化，其计算公式如下：
  $$p(x|y=y^{\ast })=\frac{p(x,y=y^{\ast })}{p(y=y^{\ast })}=\frac{p(x,y=y^{\ast })}{\int p(x,y=y\ast )dx}=p(x|y)=\frac{p(x,y)}{p(y)}$$

• 三者的关系：
  

• 概率和为1:

  • 离散型
    $$\sum _{x}p(x)=1$$
  • 连续型
    $$\int p(x)dx=1$$

• 总概率定律:

  • 离散型
    $$p(x)=\sum _{y}p(x|y)p(y)$$
  • 连续型
    $$p(x)=\int p(x|y)p(y)dy$$

• 边缘概率:

  • 离散型
    $$p(x)=\sum _{y}p(x,y)$$
  • 连续型
    $$p(x)=\int p(x,y)dy$$

• Bayes公式：
  $$p(x,y)=p(x|y)p(y)=p(y|x)p(x)$$

• 条件独立
  $$p(x,y|z)=p(x|z)p(y|z)$$
  $$p(x,y)=p(x)p(y)$$

1.2 贝叶斯定理

• 贝叶斯定理（Bayes’ Rule ）：
  • 允许我们利用已有的知识或者信念（belief）（通常是先验知识prior）帮助我们计算相关事件的概率。
    
    $$p(x,y)=p(x|y)p(y)=p(y|x)p(x)$$
    $$P(x|y)=\frac{p(y|x)p(x)}{p(y)}=\frac{likelihood\cdot prior}{evidence}=posterior$$
  • $p(x)$：可以是一个值（先验值），也可以是一个分布（先验分布）
  • $p(y|x)$：似然分布，也叫证据 (y就是新的数据 证据)
  • $p(x|y)$：后验分布，由新的数据（证据）更新之后的分布
  • 分母$p(y)$：是一个归一化常量，用于确保所有的后验概率之和为1
    $$p(x|y)=\eta \cdot p(y|x)\cdot p(x)=\frac{p(y|x)\cdot p(x)}{p(y)}$$
  • 给定背景知识e,Bayes规则变为：
    $$p(x|y,e)=\frac{p(y|x,e)\cdot p(x|e)}{p(y|e)}$$
  • 学习能力：Bayes Rule允许把新的数据证据与已有经验组合在一起，且用新的数据证据更新已经有的经验，即具有学习功能。
• 链式规则（Chain Rule）：
  $$p(x_1,x_2,...,x_K)=\prod _{i=1}^{K}p(x_i|x_1,...,x_{i-1})$$

1.2.1 基于多个证据$(z_1,...,z_n)$的状态估计(递归Bayes更新)

$$p(x|z_1,...,z_n)=\frac{p(z_n|x,z_1,...,z_{n-1})p(x|z_1,...,z_{n-1})}{p(z_n|z_1,...,z_{n-1})}$$

1.2.2 Markov假设

• $z_n独立于z_1,...,z_{n-1}$, 则有：
  $$p(x|z_1,...,z_n)=\frac{p(z_n|x)p(x|z_1,...,z_{n-1})}{p(z_n|z_1,...,z_{n-1})}$$
  $$＝\eta p(z_n|x)p(x|z_1,...,z_{n-1})=[\prod _{i=1...n}({\eta }_i\cdot p(z_i|x))]p(x)$$

1.2.3 基于动作执行的状态估计

• 执行动作u,把状态x’变为x：
  $$p(x|u,x^{\prime })$$
• 集成动作的结果
  • 连续：
    $$p(x|u)=\int p(x|u,x^{\prime })p(x^{\prime })d{x}^{\prime }$$
  • 离散：
    $$p(x|u)=\sum p(x|u,x^{\prime })p(x^{\prime })$$

1.2.4 Bayes Filter框架

• 给定条件：
  • 一组观测值z和控制数据u： d t = { u 1 , z 1 , . . . , u t , z t } d_t = \{u_1, z_1, ..., u_t, z_t\} dt​={u1​,z1​,...,ut​,zt​}
  • Sensor模型：p(z|x)
  • 动作模型：p(x|u,x’)
  • 系统状态的先验概率(Prior)：p(x)
  • z：观测
  • u：动作
  • x：状态
• 目标：
  • 估计动态系统的状态x
  • 状态的后验概率也叫做Belief：$Bel(x_t)=p(x_t|u_1,z_1,...,u_t,z_t)$
• Markov假设
  

1.2.5 Bayes Filters

$Bel(x_t)=p(x_t|u_1,z_1,...,u_t,z_t)$
$Bayes$ $=\eta p(z_t|x_t,u_1,z_t,...,u_t)p(x_t|u_1,z_1,...,u_t)$
$Markov$ $=\eta p(z_t|x_t)p(x_t|u_1,z_1,...,u_t)$
$TotalProb.$ $=\eta p(z_t|x_t)\int p(x_t|u_1,z_1,...,u_t,x_{t-1})p(x_{t-1}|u_1,z_1,...,u_t)d{x}_{t-1}$
$Markov$ $=\eta p(z_t|x_t)\int p(x_t|u_t,x_{t-1})p(x_{t-1}|u_1,z_1,...,u_t)d{x}_{t-1}$
$Markov$ $=\eta p(z_t|x_t)\int p(x_t|u_t,x_{t-1})p(x_{t-1}|u_1,z_1,...,u_{t-1},z_{t-1})d{x}_{t-1}$
$=\eta p(z_t|x_t)\int p(x_t|u_t,x_{t-1})Bel(x_{t-1})d{x}_{t-1}$

1.3 概率图模型

• 概率图模型（Probabilistic Graphical Models）
  • 有向图模型用于表示随机变量的联合分布
    
• 马尔可夫模型（Markov Models）
  • 模型假设对未来的预测仅仅依赖于最后一个观测值
    
    $$p(x_1,x_2,...,x_K)=p(x_1)\prod _{i=2}^{K}p(x_i|x_{i-1})$$
• 齐次马尔可夫模型(Homogeneous Markov Model)：所有$p(x_i|x_{i-1})$是相同的

1.4 状态空间模型

• 状态空间模型（State Space Model）
  
  • $x_i$：潜在或隐藏变量（Latent or Hidden Variables），形成马尔可夫链 (状态)
  • $z_i$：观测变量（Observations Variables）(观测值)
  • 联合概率分布
    p ( x 1 , . . . , x K , z 1 , . . . , z K ) = p ( x 1 ) { ∏ i = 2 K p ( x i ∣ x i − 1 ) } &ThinSpace; ∏ i = 1 K p ( z i ∣ x i ) p(x_1,...,x_K,z_1,...,z_K)=p(x_1) \{ \prod_{i=2}^K p(x_i|x_{i-1}) \} \, \prod_{i=1}^K p(z_i | x_i) p(x1​,...,xK​,z1​,...,zK​)=p(x1​){i=2∏K​p(xi​∣xi−1​)}i=1∏K​p(zi​∣xi​)
  • 隐马尔可夫模型(HMM: Hidden Markov Model): 隐藏变量$x_i$是离散的， 观测变量$z_i$是离散或是连续的
  • 线性动态系统（LDS: Linear Dynamical System）：隐藏变量$x_i$和 观测变量$z_i$都是连续的，且满足高斯分布
  • 状态空间模型的三个组件：
    • 变换模型(Transition Model)：描述系统是如何演变的，它定义了概率分布$p(x_i|x_{i-1})$
    • 观测模型（Observation/Sensor Model）：定义了概率分布$p(z_i|x_i)$
    • 先验概率分布（Prior Probability Distribution）：组合所有随机变量的联合分布$p(x_0)$
      
  • 状态空间模型可完成的推理任务：
    • 滤波（Filtering）：基于所有观测值计算后验概率（posterior distribution）$p(x_k|z_{1:k})$；以在线方式跟踪系统的当前状态
    • 预测（Prediction）：计算后验概率 $p(x_{k+t}|z_{1:k})t>0$
    • 平滑（Smoothing）：计算后验概率 $p(x_t|z_{1:k})0\le t<k$
    • 寻找最优状态序列（Most likely sequence）：给定一个观测序列，找到一个最有可能产生此观测序列的状态序列,如语音识别
      \begin{equation}
      \mathop{\arg\max}{x{1:k}} \ \mathrm{p}(x_{1:k} | z_{1:k})
      \end{equation}

1.5 线性动态系统(LDS)

• 线性高斯状态空间模型：Linear-Gaussian State Space Model
  • $F_k,H_K$：都是线性函数
    
• 线性高斯的特点（Linear-Gaussian）
  • 线性变换不变性：高斯分布经过线性变换之后仍然是高斯分布
    
  • 边缘分布不变性：给定高斯联合分布，所有推导出来的边缘分布仍然是高斯分布
    
  • 条件分布不变性：给定高斯联合分布，所有推导出来的条件分布仍然是高斯分布
    
  • 乘积分布不变性：两个高斯分布相乘仍然是高斯分布
    
• LDS定义：基于线性高斯假设的时间模型（Temporal Model）
• LDS表示：
  
  
  • LDS假设：所有噪声随机变量相互独立。
  • $F、G$：根据运动学进行计算（即根据运动学列出每一个状态变量的方程，然后求得$F$和$G$）
  • $H$：根据观测值与状态值间的关系列出方程，然后求出观测矩阵$H$

1.6 常用概率分布

1.6.1 高斯分布/正态分布（Gaussian Distribution）

• 连续随机变量最广泛使用的分布
• 特征：
  • 简单：仅由两个矩（参数）表示，均值(mean)和方差(variance)
  • 中心极限定理：Central Limit Theorem (CLT)
• ** 一元概率密度函数（PDF）**
  $${\mathcal{N}}_x(\mu ,{\sigma }^2)=p(x)=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}$$
  
• 多元概率密度函数（PDF）
  • $x$为D维随机向量 { x 1 , x 2 , . . . , x D } \{x_1,x_2, ..., x_D \} {x1​,x2​,...,xD​}
    N x ( μ , Σ ) = p ( x ) = 1 ∣ Σ ∣ 1 / 2 &ThinSpace; ( 2 π ) D / 2 e x p { − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) } \mathcal{N_x}(\mu, \Sigma) = p(x) = \frac {1}{|\Sigma|^{1/2}\,(2 \pi)^{D/2}} exp \{ - \frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu) \} Nx​(μ,Σ)=p(x)=∣Σ∣1/2(2π)D/21​exp{−21​(x−μ)TΣ−1(x−μ)}
    

2. 参数估计方法

• 参数估计的目的：根据观测数据，估计其概率分布的参数，使观测数据出现的概率最大。
• （统计学）推断：是从数据中推导一个概率分布性质的过程。
• 参数估计的步骤：
  • 根据观测值，确定数据分布的模型
  • 用有效的方法求模型的参数
  • 参数：在机器学习中，通常用一个模型来描述我们观测到的数据，每个模型都有一组自己的参数，用于决定模型的质量，如高斯（正态）分布的参数为$(\mu ,\sigma )$
• 数据滤波：是$去除噪声还原真实数据$的一种数据处理技术
• 状态量：受噪声干扰的状态量是个随机量，不可能测得精确值，但可对它进行一系列观测，并依据一组观测值，按某种统计观点对它进行估计。使估计值尽可能准确地接近真实值，这就是最优估计。真实值与估计值之差称为估计误差。若估计值的数学期望与真实值相等，这种估计称为无偏估计。
• 状态估计：一般来说，根据观测数据对随机量进行定量推断就是估计问题，特别是对动态行为的状态估计，它能实现实时运行状态的估计和预测功能。比如对飞行器状态估计。状态估计对于了解和控制一个系统具有重要意义，所应用的方法属于统计学中的估计理论。
  • 最常用的估计方法：
    • 最小二乘估计
    • 线性最小方差估计
    • 最小方差估计
    • 递推最小二乘估计
  • 基于风险准则的估计方法：
    • 贝叶斯估计
    • 最大似然估计
    • 随机逼近
• 高斯白噪声（White Gaussian Noise）：即噪声与时间和空间是没有关系的，而且符合相同的高斯分布（Gaussian Distribution）

2.1 极大似然估计（MLE: Maximum Likelihood Estimation）

• MLE用途：是估计模型参数的一种方法，通过最大化观测数据的总概率得到最终的参数值。
• MLE原理：$因为已经观测到这些样本数据，所以这些样本数据的概率应该极大（最大）$，即最大化观测数据的总概率。（即给定一系列观测数据点，我们进行极大似然估计得到参数的估计，相当于得到关于数据集中这个参数变量的均值信息）
• MLE求解步骤：
  • 似然函数（Likelihood function）：给定观测数据，模型参数取特定值的可能性
  • 写出似然函数：$L(\theta) = L(x_1,x_2,…,x_n; \theta_1, \theta_1, …, \theta_m),x_i为观测数据，\theta_j为模型参数 $；观测数据的似然函数$L(\theta)$是$\theta$的函数，随$\theta$的变化而变化
  • 对似然函数取自然对数：即对数似然（Log Likelihood），用于去指数，把乘法变成加法，把除法变成减法，以方便后面求导 ；因为$ln(x)$是单调函数，所以$ln(x)$与$L(\theta )$的极值点位于相同的位置）
  • 对各参数求偏导，令导函数为0，组成似然方程组
  • 解似然方程组，求得各个模型参数($此模型使得观测到这些样本数据的概率最大$)
• 离散型似然函数
  • 若X为离散型， 其概率分布为：$P(X=x)=p(x;\theta )$ (分号后面的为参数)
  • $X$取到观测值$x_1,x_2,...,x_n$的概率为：$$L(\theta )=L(\theta ;x_1,...,x_n)=\prod _{i=1}^{n}p(x_i;\theta )$$
• 连续型似然函数
  • 若X为连续型， 其概率密度函数为：$f(x;\theta )$ (分号后面的为参数)
  • $X$取到观测值$x_1,x_2,...,x_n$的概率为：$$L(\theta )=L(\theta ;x_1,...,x_n)=\prod _{i=1}^{n}f(x_i;\theta )$$

2.1.1 参数的极大似然与数据的极大概率

• 极大似然(Maximum Likelihood) ：$L(\theta ;data)$，给定观测数据，得到$参数的似然$
• 极大概率(Maximum Probability)：$p(data;\theta )$，给定参数，得到$观测数据的概率$
• 二者关系
  $$L(\theta ;data)=p(data;\theta )$$

2.1.2 极大似然估计与最小二乘法

• 最小二乘法（Least Squares）: 在机器学习中，常用于估计模型参数的方法
• 若模型是高斯的，则极大似然估计与最小二乘法等价，证明
• 可以通过理解两个方法的目标来解释他们。对最小二乘估计，我们想要找到一条线，来最小化全部数据点和回归线的平方距离（可以看下面的图）。对于极大似然估计，我们想要最大化数据的总概率。当假设为高斯分布时，数据点离均值近的时候得到最大概率。由于高斯分布是对称的，这等价于最小化数据点和均值之间的距离。

2.2 贝叶斯推断进行参数估计(MAP估计)

• 贝叶斯推断(BI)进行参数估计：Bayesian Inference for Parameter Estimation
• 用途：贝叶斯推断是给定观测数据，利用贝叶斯定理来推导概率分布的性质。
• 原理：先验分布通过更新证据（evidence），从而计算出后验分布，然而求出后验分布的MAP。
• 计算公式：$后验分布＝似然分布\ast 先验分布$
• BI与MLE的关系：BI可视为MLE的一般化方法。
• 待估参数：我们通常用$\Theta$表示感兴趣的事件，它表示一组参数；下面以估计高斯分布的参数为例，则$\Theta$表示高斯分布的均值$\mu$和标准差$\theta$（公式表示为： Θ = { μ , θ } \Theta = \{ \mu, \theta \} Θ={μ,θ}）
• 观测数据：以data表示， d a t a = { y 1 , y 2 , . . . , y n } data=\{ y_1, y_2, ..., y_n\} data={y1​,y2​,...,yn​}
• 按贝叶斯定理，可写成如下公式：
  $$p(\Theta |data)=\frac{p(data|\Theta )p(\Theta )}{p(data)}$$
  • $p(\Theta )$：先验分布，表示我们根据经验对参数值的估计。
  • $p(\Theta |data)$：后验分布，表示每次用观测数据更新先验分布获得的新分布。
  • $p(data|\Theta )$：似然分布，它与极大似然估计中的似然函数$L(\Theta ;data)$类似（极大似然等于极大概率），有时也被称为证据（evidence）。
  • $p(data)$：并不参与概率分布的计算，它只是一个数值。$p(data)$的值可以通过观测数据获得，其存在的意义在于使$p(\Theta |data)$所有可能的总和或积分为1。

2.2.1 实例

• 三个分布如下图所示：
  
• 蓝色分布：先验分布(prior distribution)，μ=3.6，标准差σ=0.2
• 金色分布：根据５个金色数据点产生的似然分布(likelihood distribution)
• 粉色分布：由先验分布和似然分布相乘产生的后验分布(posterior distribution)
• 后验分布最常见的统计数据之一是mode。这通常被用作估计感兴趣的参数，被称为最大后验概率估计（Maximum a posterior probability estimate）或者简单地称为MAP估计。

2.2.2 工作原理

• 你对某物（例如参数的值）有一个先验belief，然后接收一些数据。你可以根据上述公式来计算后验分布从而更新你的belief。之后，我们得到更多的数据，后验成为了新的先验。我们可以用新数据来更新新的先验，并且再次得到新的后验。这个循环可以无限持续，所以能不断更新你的beliefs。
• 先验$\to$新的数据$\to$后验$\to$新的先验$\to$新的数据$\to$后验
• 卡尔曼滤波器（以及它的变体）就是一个很好的例子。它被用在很多情况下，但是可能在数据科学领域最重要的应用是自动驾驶汽车。

2.2.3 MAP估计与极大似然估计

• 当先验分布为均匀分布时(即先验分布为常数,与样本无关；直观地说，它表示缺乏先验知识)，MAP估计等于MLE（极大似然估计）。
• 极大似然估计(MLE)可以看作是MAP估计的一个特例（没有先验知识的特例）

2.3 卡尔曼滤波器（Kalman Filter）

• 定义：卡尔曼滤波器是一种最优估计器，即可以从间接、不准确和不确定的观测数据中推断感兴趣的参数。
• 卡尔曼滤波器的分类：

类别	特征
KF	要求是一个线性模型
EKF	是一个非线性模型，通过Taylor展开进行局部线性化，在均值附件线性化
UKF	是一个非线性模型，通过无损变换UT(Unscented Transforms)进行局部线性化，但其近似度更高

• 理论基础：基于贝叶斯推断进行参数估计，寻找估计参数的方法为参数的均方误差最小化。
• 基本假设：
  • 后验概率分布为高斯分布
  • 线性系统
  • 高斯白噪声
• 原理：如果所有噪声符合高斯分布，则卡尔曼滤波最小化估计参数（estimated parameters）的均方误差（MSE: Mean Square Error）$\sigma$ ，即最大化其确定性。
• 为什么叫滤波器：它从有噪声的数据中找到最好的估计，以达到滤除噪声的目的
  • 滤波实例
• 特性：
  • 采用状态空间描述法
  • 线性估计
  • 算法采用递归形式 (新的数据可以被实时处理，以获得新的状态 )
  • 能处理多维和非平稳的随机过程
• 用途：Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。对于解决大部分的问题，他是最优，效率最高甚至是最有用的。它广泛应用于：
  • 机器人导航
  • 控制
  • 传感器数据融合
  • 军事方面的雷达系统以及导弹追踪
  • 计算机图像处理（如人脸识别，图像分割，图像边缘检测等等）
• 高斯随机变量线性变换特性
  • 高斯随机变量经过线性变换之后，仍然是高斯随机变量
  • 线性变换之后的均值和协方差如下：
    
    
  • $A{\Sigma }_x{A}^T$：叫做误差传播定律（Error Propagation Law）
• 状态和状态协方差公式
  
  
  • ${\mathbf{X}}_k和P_k$：分别为状态和状态的协方差
  • $K_k$：为卡尔曼增益矩阵

2.3.1 KF (Kalman Filter 线性)

• ${\mathbf{x}}_k和{\mathbf{z}}_k$：都是线性组合，即是其它变量的线性组合
• $\mathbf{x}(k|k)、P(k|k)$：$k$时刻的状态及状态协方差，它基于所有至$k$时刻的观测数据 (“prior”)
• $\mathbf{x}(k+1|k)、P(k+1|k)$：$k+1$时刻的状态及状态协方差，它基于所有至$k$时刻的观测数据 (“prediction”)
• $\mathbf{x}(k+1|k+1)、P(k+1|k+1)$：$k+1$时刻的状态及状态协方差，它基于所有至$k+1$时刻的观测数据 (“posterior”)
  
  • $\nu$：表示真正的观测值与预测的观测值之差
  • $H_k$：经常用于表示一个简单的坐标系变换，如预测值${\mathbf{x}}_k$是odom坐标系，而观测值${\mathbf{z}}_k$是LaserScan局部坐标系。
  • 根据观测值计算$R$：
    
• 卡尔曼滤波器循环（Kalman Filter Cycle）
  
  • 测量预测（Measurement Prediction）：经常是坐标系变换（coordinate frame transform），状态是基于全局/世界坐标系，而观测是基于局部Sensor坐标系。
  • 数据关联（Data Association）：如果有多个待估计的状态或观测值，需要把正确的预测值和观测值进行配对，即一个预测值与哪一个观测值相对应，以输出正确的结果。通过统计兼容性测试来判断预测值$i$与观测值$j$之间的配置是正正确的。
    

2.3.2 EKF (Extended Kalman Filter 非线性)

• ${\mathbf{x}}_k或{\mathbf{z}}_k$：是非线性组合

• 对于非线性变换，其输入为高斯分布，其输出不再为高斯分布

• 通过一阶Taylor展开进行近似

• 非线性函数一阶误差传播（一维变量和多维变量）
  

• EKF公式
  

2.3.3 UKF (Unscented Kalman Filter 非线性)

• 定义：对非线性模型一种效果更好的线性化方式（与Taylor展开比较）
• 实现方法：
  • 计算一个Sigma点集合
    
  • 通过非线性函数变换每个Sigma点 (所以叫做无损变换UT: Unscented Transform)
    
  • 根据变换后的点及对应的权重计算高斯分布 (而不像EFK哪样在均值附近进行局部线性化)
    
• EKF变换过程如下：
  
• Sigma点(${\chi }^{[i]},{\omega }^{[i]}$)的符合以下要求，且${\chi }^{[i]},{\omega }^{[i]}$没有唯一解
  
• 计算Sigma点 ${\chi }^{[i]}$
  
• 计算权重${\omega }^{[i]}$
  
• 计算要求的高斯分布
  
• 无损变换参数
  

2.3.4 Particle Filter（粒子滤波）

• 如果数据的分布是任意形式的，则不能采用高斯模型假设
• 使用基于样本的不确定性表示，粒子过滤器（PF）是递归贝叶斯过滤器的实现，不对底层分布和系统模型做任何假设

3. 数学基础

3.1 概率论与统计学的区别

• 概率论：研究一个$白盒$，即知道盒子内的详细内容（如有几个白球，有几个黑球，也就是常说的概率分布），然后计算下一个摸出白球的概率。
• 统计学：研究一个$黑盒$，即不知道盒子内的详细内容，只知道每次摸出来的是白球或黑球，其主要任务是根据实验结果，来估计盒子内的详细内容。
• 统计推断：做统计推断一般都需要对那个黑盒子做各种各样的假设（即它符合什么分布），这些假设都是概率模型，统计推断就是$估计这些模型的参数$。

3.2 一维离散随机变量（均值、方差、标准差）

• 描述对象：一个随机变量（即一个特征）的多个样本
• 样本（抽样）：即有限个实验样本，其样本数为n
• 总体：所有样本（即接近于随机变量本身的概率分布），设总体数为N(接近于无穷大)
• 样本方差中为什么除以$n-1$?
  • 使得样本方差更能反映总体方差，因为样本范围内可能不包含总体均值，所以样本方差如果以样本个数n作为分母，就往往比总体方差小一点。
    | 概念 |总体（概率分布） | 样本 |
    | ------------- |:-------------? -----?
    | 均值(mean) | $\mu=\frac{1}{N}\sum_{i=1}^NX_i $ | $\overline{x}=\frac{1}{n}{\sum }_{i=1}^n{x}_i$ |
    | 方差(variance) | ${\sigma }^2=\frac{1}{N}{\sum }_{i=1}^N(X_i-\mu {)}^2$ | 总体样本的无偏见估计$S^2=\frac{1}{n-1}{\sum }_{i=1}^n(x_i-\bar{x}{)}^2$ |
    | 标准差(standard deviation) | $\sigma =\sqrt{{\sigma }^2}=\sqrt{\frac{1}{N}{\sum }_{i=1}^N(X_i-\mu {)}^2}$ | $S=\sqrt{S^2}=\sqrt{\frac{1}{n-1}{\sum }_{i=1}^n(x_i-\bar{x}{)}^2}$|
    | 协方差(covariance) |$Cov(X,Y)=\frac{1}{N}{\sum }_{i=0}^N(X-{\mu }_x)(Y-{\mu }_y)$ |$Cov(x,y)=\frac{1}{n-1}{\sum }_{i=1}^n(x_i-\bar{x})(y_i-\bar{y})$ |
    | 相关系数（correlation coefficient） |${\rho }_{X,Y}=\frac{Cov(X,Y)}{{\sigma }_X{\sigma }_Y}$ |${\rho }_{x,y}=\frac{Cov(x,y)}{S_x{S}_y}$ |
    | 协方差矩阵(covariance matrix) | | |

3.2.1 数学期望(Expectation)

• 别名：数学期望或期望 （Expectation)
• 物理意义：描述一个特征（即一个随机变量）的$平均水平$，需要多个样本才有意义，但不能反映样本个体情况
• E(随机变量)的计算公式：在概率论和统计学中，均值(mean)（或数学期望，亦简称期望）是试验中每次可能结果的概率乘以其结果的总和。严格的定义如下：
  $$\mu =E[x]=\sum _{i=0}^{\infty }{x}_i\cdot p_i\approx \frac{1}{N}\sum _{i=1}^N{x}_i(X是离散随机变量)$$
  • $x_i$：表示一维随机变量X的一个观测样本值
  • $p_i$：表示$x_i$发生的概率
    $$\mu =E[x]=\int x\cdot p(x)dx(X是连续随机变量)$$
  • $p(x)$：随机变量$X$的概率密度函数(PDF)
• E(随机变量的函数)的计算公式:
  $$E[f(x)]=\sum f(x)\cdot p(x)(x是离散随机变量)$$
  $$E[f(x)]=\int f(x)\cdot p(x)dx(x是连续随机变量)$$
  $$E[f(x,y)]=\iint f(x,y)\cdot p(x,y)dxdy(x,y是连续随机变量)$$
• 各种常用数学期望的专用名称
  
• 数学期望（均值）和算术平均值（平均数）的关系
  • 平均数：是根据实验结果统计得到的随机变量样本计算出来的算术平均值，$和实验本身有关$，其计算公式为：
    $$\overline{x}=\frac{1}{n}\sum _{i=1}^n{X}_i$$
  • 数学期望（均值）：是完全由随机变量的概率分布所确定的，$和实验本身无关$；
  • 二者的关系：实验的次数多少是可以改变平均数的，而在你的分布不变的情况下，期望是不变的。 如果我们能进行无穷次随机实验并计算出其样本的平均数的话，那么这个平均数其实就是期望。
• 性质
  • 以下设$a$为常数，$x、y$为随机变量
  • $E[a]=a$
  • $E[a\cdot f(x)]=aE[f(x)]$
  • $E[f(x)+f(y))=E[f(x)]+E[f(y)]$
  • $x与y相互独立，E[f(x)\cdot f(y)]=E[f(x)]\cdot E[f(y)]$

3.2.2 方差(variance)

• 方差：描述一个特征（即一个随机变量）的$离散程度$，即描述样本集合中的各个样本点到均值的距离的平均。
• 物理意义：在概率论中，方差用来度量随机变量和其数学期望（即均值）之间的偏离程度。
• 计算公式：
  $${\sigma }^2=D[x]=Var[x]=E[(x-E[x]{)}^2]\approx \frac{1}{N}\sum _{i=1}^N(x_i-{\mu }_x{)}^2(x为离散随机刘变量是)$$
  $${\sigma }^2=D[x]=Var[x]=E[(x-E[x]{)}^2]=\int (x-{\mu }_x{)}^2\cdot p(x)dx(x为连续随机刘变量是)$$
  $$Var(x)=E[x^2]-(E[x]{)}^2$$
  • $x$：表示一维随机变量
  • $E[x]$：表示$x$的均值(即期望）
  • $D[x]、Var[x]$：是每个样本值与全体样本均值之差的平方的平均数。
• 方差的性质：
  • 以下设a为常数，x、y为随机变量
  • $D[a]=0$
  • $D[ax]=a^{2}D[x]$
  • $D[x+a]=D[x]$
  • $D[x\pm y]=D[x]+D[y]\pm 2Cov(x,y)$
  • $x与y相互独立：D[x+y]=D[x]+D[y]$
  • $D[x]=0的充要条件是x以概率1取常数E[x],即P(x=E[x])=1$
  • $D[ax+by]=a^{2}D[x]+b^{2}D[y]+2abCov(x,y)$

3.2.3 标准差/均方差

• 物理意义：反映一个数据集的离散程度
• 计算公式：
  $$\sigma =\sqrt{D[x]}$$
• 标准差比方差的优势：
  • 和原始数值的单位一致，不需要使用单位的平方
  • 标准差可以计算钟型曲线（正态分布）的中心值临近区间的概率值，根据正态分布定义，中心值的正负n倍 $\sigma$ 构成的区间对应不同的概率值．在正态分布中，正负一个标准差之内所占比率为全部数值之 68。2%；正负两个标准差之内（深蓝，蓝）的比率合起来为 95.4%；正负三个标准差之内（深蓝，蓝，浅蓝）的比率合起来为 99.6%。
    
  • 在大样本中一般使用样本的标准差近似代替总体的标准差，尽管样本标准差并不是理论上的无偏值，小样本中偏差会比较大，但仍然可以通过t分布模型等方法去估算。

3.2 多维随机变量（协方差、相关系数、协方差矩阵）

• 均值、方差、标准差：描述了一个随机变量多个样本的统计属性，若一个对象有多个特征（多个随机变量），如何描述多个随机变量之间的关系呢？这正是协方差的用武之地。

3.2.1 协方差

• 协方差用途：用于描述两个随机变量在变化过程中是同方向变化，还是反方向变化？同向或反向的程度如何？
  • 在你变大的时刻，我也变大，说明两个随机变量是同向变化的，这时协方差是正的。
  • 在你变大的时刻，我变小，说明两个随机变量是反向变化的，这时协方差就是负的。
  • 从数值来看，协方差的数值越大，两个随机变量同向程度也就越大，在本质上，协方差并不能很好地描述同向或反向程度如何，而相关系数反而能更好地描述同向或反向的程度。
• 协方差公式
  $$Cov(X,Y)=E[(X-E(X))(Y-E(Y))]\approx E[(X-{\mu }_x)(Y-{\mu }_y)]=\frac{1}{N}\sum _{i=0}^N(X-{\mu }_x)(Y-{\mu }_y)$$
• $X、Y$：为两个随机变量
• 若$X和Y$为同一个随机变量，则$Cov(X,Y)$为$X$的方差，即$Cov(X,Y)={\sigma }^2$
• 同向变化
  
  • 从上图可知，$X、Y$均沿着各自的均值上下波动，且变化明显是同向的
  • 在任意时刻$t_i$，$(X-{\mu }_x)与(Y-{\mu }_y)$正负符号相同，所以其乘积为正；然后所有乘积相加，其结果仍然为正
• 反向变化
  
  • 从上图可知，$X、Y$均沿着各自的均值上下波动，且变化明显是反向的
  • 在任意时刻$t_i$，$(X-{\mu }_x)与(Y-{\mu }_y)$正负符号相反，所以其乘积为负；然后所有乘积相加，其结果仍然为负
• 在实际应用中，样本数据不可能这么有规律，会出现$(X-{\mu }_x)与(Y-{\mu }_y)$时而为正，时而为负，我们关注的重点是总体样本，而不是个体样本，只要其最后的$Con(X,Y)$为正，则表明$X、Y$的同向变化是大趋势，且值(正数)越大，则同向程度越高；若为负，则表明$X、Y$的反向变化是大趋势，且值(负数)越小，则反向程度越高。

3.2.2 相关系数

• 相关系数公式
  $${\rho }_{X,Y}=\frac{Cov(X,Y)}{{\sigma }_X{\sigma }_Y}$$

  • 即用$X、Y$的协方差除以X的标准差和Y的标准差。
  • 相关系数：可以看作是经过归一化之后特殊协方差
  • 即值范围：$[-1，1]$
  • 反映两个随机变量变化时是同向还是反向变化，如果同向变化就为正，反几变化变为负。
  • 它消除了两个随机变量变化幅度的影响，从而单纯地反映两个随机变量每单位变化时的相似程度。

• 相关系数用途
  

  • 从上图可知，情况1与情况2在任意时刻都是同向变化，且均值相同
  • 很明显，情况1的协方差大于情况2的协方差，因为情况1中的$(X-{\mu }_x)$大于情况2中的$(X-{\mu }_x)$
  • 从本质上分析，情况1与情况2的同向程度相同，但协方差却不一样，所以协方差描述同向程度是不准确的，因为它受变化幅度的影响
  • 而相关系数${\rho }_{x,y}$却是相同的，所以相关系数能很好地刻画同向程度，剔除了变化幅度的影响

• 协方差与相关系数

  • 协方差受变化幅度的影响
  • 相关系数不受变化幅度的影响，能很好地描述同向或反向程度

• 相关系统各个值域的含义

  • 相关系数为1：表明两个随机变量变化时的正向相似度最大，二者变化的倍数（即$Y=aX且a>0$）相同（你变大一倍，我也变大一倍；你变小一倍，我也变小一倍），即完全相关（以X、Y为横纵坐标轴，可以画出一条斜率为正的直线，所以X、Y是线性关系的）
  • 相关系数为[0,1)：随着相关系数减小，两个随机变量变化时的相似度也变小，当为0时，两个变量的变化过程没有任何相似度，即变量无关。
  • 相关系数为(-1,0]：当相关系数小于0，两个随机变量开始出现反向的相似度，随着相关系数减小，反向相似度会逐渐变大
  • 相关系数为-1：表明两个随机变量变化时的反向相似度最大，二都变化的倍数相反($Y=bX且b<0$），即你变大一倍，我变小一倍；你变小一倍，我变大一倍。即完全负相关（以X、Y为横纵坐标轴，可以画出一条斜率为负的直线，所以X、Y是线性关系的）

3.2.3 协方差矩阵

• 两个随机变量间可以计算其协方差，以描述其同向或反向程度，若一个对象包含有$n$个随机变量，如何描述其任意两个随机变量的协方差呢？ 答案是$协方差矩阵$。

• 描述对象：协方差矩阵计算的是不同维度(即不同随机变量)之间的协方差，而不是不同样本之间的。

• 定义：

  • 样本有n个随机变量，协方差矩阵就是这n个随机变量中任意两个随机变量的协方差组成的矩阵
  • 矩阵中的数据按行排列与按列排列求出的协方差矩阵是不同的，这里默认数据是按行排列。即每一行是一观察样本(observation sample)，那么每一列就是一个随机变量。
  • 记$X_{m\times n}$：为有m个样本，且每个样本有n个随机变量的矩阵:
    $$X_{m\times n}=\left[\begin{array}{cccc}{a}_{11}& a_{12}& \cdots & a_{1n}\\ a_{21}& a_{22}& \cdots & a_{2n}\\ ⋮& ⋮& \ddots & ⋮\\ a_{m1}& a_{m2}& \cdots & a_{mn}\end{array}\right]=\left[\begin{array}{cccc}{c}_1& c_2& \cdots & c_n\end{array}\right]$$
  • $c_1$：为第一列随机变量的$m$个样本值，它是一个列向量，且为$c_{m\times 1}$
  • 协方差矩阵定义为：
    $$\sum =covMatrix=\frac{1}{m-1}\left[\begin{array}{cccc}Cov(c_1,c_1)& Cov(c_1,c_2)& \cdots & Cov(c_1,c_n)\\ Cov(c_2,c_1)& Cov(c_2,c_2)& \cdots & Cov(c_2,c_n)\\ ⋮& ⋮& \ddots & ⋮\\ Cov(c_n,c_1)& Cov(c_n,c_2)& \cdots & Cov(c_n,c_n)\end{array}\right]$$
  • 协方差矩阵的维度等于样本中随机变量的个数，即每一个样本的维度。在某些场合前边也会出现 1 / m，而不是 1 / (m - 1)。

• 计算方法：先让样本矩阵（矩阵的一行为一个样本）中心化(矩阵记作$X_D$)，即每一维度减去该维度的均值，使每一维度上的均值为0，然后直接用中心化的样本矩阵的转置($X^T$)乘上它自己($X$)，然后除以样本数(N)即可。
  $$X_D=X_{Dm\times n}=\left[\begin{array}{cccc}c{d}_1& c{d}_2& \cdots & c{d}_n\end{array}\right]$$
  $$\overline{x_j}=\frac{1}{m}\sum _{i=1}^m{a}_{i,j}c{d}_j=\left[\begin{array}{c}{a}_{1,j}-\overline{x_j}\\ a_{2,j}-\overline{x_j}\\ ⋮\\ a_{m,j}-\overline{x_j}\end{array}\right]$$
  $$\sum =covMatrix=\frac{1}{m-1}{X}_D^T{X}_D$$

• 性质

  • 协方差矩阵始终是一个对称矩阵，其对角线上是方差，非对角线上是协方差。
  • 协方差矩阵对角线上的元素其实就是对应随机变量的方差：$Cov(X,X)=Var(X)={\sigma }^2$
  • 对于随机变量$X,Y$, 则有$Cov(X,Y)=Cov(Y,X)$

• 协方差矩阵定义数据形状
  

  • 协方差矩阵定义了我们数据的传播（方差）和方向（协方差）。

###2.2.4协方差矩阵的特征值分解

• 协方差矩阵定义了我们数据的传播（方差）和方向（协方差）。因此，如果我们想用一个向量和它的大小来表示协方差矩阵，我们应该简单地尝试找到指向数据最大传播方向上的向量，其大小等于这个方向上的传播（方差）。
• 协方差矩阵的最大特征向量总是指向数据最大方差的方向，并且该向量的幅度等于相应的特征值。第二大特征向量总是正交于最大特征向量，并指向第二大数据的传播方向。
• 协方差矩阵是对角矩阵:
  • 当协方差是零时，方差必须等于特征值λ。如下图所示，特征向量用绿色和品红色表示，特征值显然等于协方差矩阵的方差分量。
    
• 协方差矩阵不是对角矩阵
  • 当协方差不为零，那么情况稍微更复杂一些。特征值仍代表数据最大传播方向的方差大小，协方差矩阵的方差分量仍然表示x轴和y轴方向上的方差大小。但是，因为数据不是轴对齐的，所以这些值不再与上图所示的相同。
    
• 特征值：表示沿特征向量方向数据的方差，而协方差矩阵的方差分量表示沿轴的传播。如果没有协方差，则这两个值是相等的。
• 特征向量表示数据最大方差的方向，特征值表示特征向量方向方差的幅度。
• 协方差矩阵与白色不相关数据的线性变换有直接的关系，此线性变换完全由数据的特征向量和特征值确定。而特征向量表示旋转矩阵，特征值对应于每个维度上缩放因子的平方。
  

[参考]
http://ais.informatik.uni-freiburg.de/teaching/ss18/robotics/slides
http://ais.informatik.uni-freiburg.de/teaching/ss11/robotics/slides/09b-errorprop.pdf
EKF Tutorial (直观理解，浅显易懂)
Unscented Kalman Filter