状态估计中的概率基础

1 概率密度函数 PDF

请注意公式⾥⾯的 p(x) 是概率密度（probability density）⽽不是概率（probability）。
$${\int }_a^{b}p(x)\mathrm{d}x=1$$

PDF的积分为概率:
$$\mathrm{Pr}(c\le x\le d)={\int }_c^{d}p(x)\mathrm{d}x$$

条件概率, $x\in [a,b]$ 在 $y\in [c,d]$的条件下的PDF如下
$$(\forall y){\int }_a^{b}p(x\mid y)\mathrm{d}x=1$$

x, y 联合概率密度函数（joint probability densities）
$$p(\mathbf{x},\mathbf{y})$$

2 贝叶斯推断 Bayesian Inference

2.1 贝叶斯公式 Bayes’ Rule

关键的就是两条公式
$$p(\mathbf{x},\mathbf{y})=p(\mathbf{x}\mid \mathbf{y})p(\mathbf{y})=p(\mathbf{y}\mid \mathbf{x})p(\mathbf{x})$$

变形之后就是贝叶斯公式，这条公式在状态估计中会被反复使用，是最核心的公式，公式本身没有任何假设。
$$p(\mathbf{x}\mid \mathbf{y})=\frac{p(\mathbf{y}\mid \mathbf{x})p(\mathbf{x})}{p(\mathbf{y})}$$

关于这个公式，每个PDF有个通俗的叫法

• P(x|y) Posteriori PDF
• P(y|x) 传感器观测模型，Measurement PDF,
• P(x) Prior PDF

贝叶斯公式的直观理解：使用先验PDF和观测PDF来推断后验的PDF，有了后验的PDF我们就知道当前应该选择的最佳估计值，以及估计值对应的不确定程度（方差）。

2.2 贝叶斯推断Bayesian Inference

• 输入:先验P(x), 观测P(y|x)
• 输出:后验P(x|y)

将 P(y) 展开得到
$$p(\mathbf{x}\mid \mathbf{y})=\frac{p(\mathbf{y}\mid \mathbf{x})p(\mathbf{x})}{p(\mathbf{y})}=\frac{p(\mathbf{y}\mid \mathbf{x})p(\mathbf{x})}{\int p(\mathbf{y}\mid \mathbf{x})p(\mathbf{x})\mathrm{d}\mathbf{x}}$$

P(y)的计算方式就是把x通过积分来边缘化掉的。关于边缘化可以参看文章

2.3 PDF连续状态的特性：矩 Moments

• 一阶矩：expectation/mean，期望
  $$\mathbf{\mu }=E[\mathbf{x}]=\int \mathbf{x}p(\mathbf{x})\mathrm{d}\mathbf{x}$$
• 二阶矩：covariance 协方差矩阵
  $$\mathbf{\Sigma }=E\left[(\mathbf{x}-\mathbf{\mu })(\mathbf{x}-\mathbf{\mu }{)}^{\mathrm{T}}\right]$$
• 三阶矩：skewness 偏度
• 四阶矩：kurtosis 峰度

Moments的期望和方差是一个PDF的关键特征，理论上可以定义无限阶次矩，通过无限阶次矩来恢复PDF。
通常在状态估计中我们只用期望和协方差来近似描述一个PDF最佳估计值所在的点。
特别的对于高斯分布，只是用期望和协方差便可以完整地描述整个PDF。

2.4 PDF离散状态的描述：均值和方差

对应于连续状态PDF的期望和协方差
$$\begin{array}{rl}{\mathbf{\mu }}_{\text{meas }}& =\frac{1}{N}\sum _{i=1}^N{\mathbf{x}}_{i,\text{ meas }}\\ {\mathbf{\Sigma }}_{\text{meas }}& =\frac{1}{N-1}\sum _{i=1}^N\left({\mathbf{x}}_{i,\text{ meas }}-{\mathbf{\mu }}_{\text{meas }}\right){\left({\mathbf{x}}_{i\text{, meas }}-{\mathbf{\mu }}_{\text{meas }}\right)}^{\mathrm{T}}\end{array}$$

2.5 归一化积：融合对同⼀个随机变量的多次估计

• 输入: P(x|y1), P(x|y2), P(x|y3), …, P(x|yn)
• 输出：P(x|y1, y2, y3, …, yn)
• 假设：y1,y2,y3, …, yn相互独立

根据贝叶斯理论，归⼀化积可⽤于融合对同⼀个随机变量的多次估计。
x为估计变量，y1，y2是两次独立的测量，
$$p\left({\mathbf{y}}_1,{\mathbf{y}}_2\mid \mathbf{x}\right)=p\left({\mathbf{y}}_1\mid \mathbf{x}\right)p\left({\mathbf{y}}_2\mid \mathbf{x}\right)=\frac{p\left(\mathbf{x}\mid {\mathbf{y}}_1\right)p\left({\mathbf{y}}_1\right)}{p(\mathbf{x})}\frac{p\left(\mathbf{x}\mid {\mathbf{y}}_2\right)p\left({\mathbf{y}}_2\right)}{p(\mathbf{x})}$$

整理得到
$$\begin{array}{c}p\left(\mathbf{x}\mid {\mathbf{y}}_1,{\mathbf{y}}_2\right)=\eta p\left(\mathbf{x}\mid {\mathbf{y}}_1\right)p\left(\mathbf{x}\mid {\mathbf{y}}_2\right)\\ \eta =\frac{p\left({\mathbf{y}}_1\right)p\left({\mathbf{y}}_2\right)}{p\left({\mathbf{y}}_1,{\mathbf{y}}_2\right)p(\mathbf{x})}\end{array}$$

3 高斯分布

高斯分布是一个理想的分布，在真实世界中，模型往往是非线性非高斯的，为了方便处理，我们经常在使用高斯分布来近似表示真实世界中的PDF的某个点。

3.1 Gaussian PDF

$$p\left(x\mid \mu ,{\sigma }^2\right)=\frac{1}{\sqrt{2\pi {\sigma }^2}}\exp \left(-\frac{1}{2}\frac{(x-\mu {)}^2}{{\sigma }^2}\right)$$
$$p(\mathbf{x}\mid \mathbf{\mu },\mathbf{\Sigma })=\frac{1}{\sqrt{(2\pi {)}^N\det \mathbf{\Sigma }}}\exp \left(-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })\right)$$

习惯上，我们也将正态分布（即⾼斯分布）记为:
$$\mathbf{x}\sim \mathcal{N}(\mathbf{\mu },\mathbf{\Sigma })$$

标准正态分布（standard normally distributed）:
$$\mathbf{x}\sim \mathcal{N}(0,1)$$

3.2 高斯推断（贝叶斯推断在高斯分布的一次实现）

$$\begin{array}{rl}p(\mathbf{x},\mathbf{y})& =p(\mathbf{x}\mid \mathbf{y})p(\mathbf{y})\\ p(\mathbf{x}\mid \mathbf{y})& =\mathcal{N}\left({\mathbf{\mu }}_x+{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}\left(\mathbf{y}-{\mathbf{\mu }}_y\right),{\mathbf{\Sigma }}_{xx}-{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}{\mathbf{\Sigma }}_{yx}\right)\\ p(\mathbf{y})& =\mathcal{N}\left({\mathbf{\mu }}_y,{\mathbf{\Sigma }}_{yy}\right)\end{array}$$

推导过程使用的shur complement，将联合概率分布带入gaussian PDF展开，只关注二次项部分

$$p(\mathbf{x},\mathbf{y})=\mathcal{N}\left(\left[\begin{array}{l}{\mathbf{\mu }}_x\\ {\mathbf{\mu }}_y\end{array}\right],\left[\begin{array}{ll}{\mathbf{\Sigma }}_{xx}& {\mathbf{\Sigma }}_{xy}\\ {\mathbf{\Sigma }}_{yx}& {\mathbf{\Sigma }}_{yy}\end{array}\right]\right)$$

$$\begin{array}{rl}& {\left(\left[\begin{array}{l}\mathbf{x}\\ \mathbf{y}\end{array}\right]-\left[\begin{array}{l}{\mathbf{\mu }}_x\\ {\mathbf{\mu }}_y\end{array}\right]\right)}^{\mathrm{T}}{\left[\begin{array}{ll}{\mathbf{\Sigma }}_{xx}& {\mathbf{\Sigma }}_{xy}\\ {\mathbf{\Sigma }}_{yx}& {\mathbf{\Sigma }}_{yy}\end{array}\right]}^{-1}\left(\left[\begin{array}{l}\mathbf{x}\\ \mathbf{y}\end{array}\right]-\left[\begin{array}{l}{\mathbf{\mu }}_x\\ {\mathbf{\mu }}_y\end{array}\right]\right)\\ & ={\left(\left[\begin{array}{l}\mathbf{x}\\ \mathbf{y}\end{array}\right]-\left[\begin{array}{l}{\mathbf{\mu }}_x\\ {\mathbf{\mu }}_y\end{array}\right]\right)}^{\mathrm{T}}\left[\begin{array}{cc}1& 0\\ -{\mathbf{\Sigma }}_{yy}^{-1}{\mathbf{\Sigma }}_{yx}& 1\end{array}\right]\left[\begin{array}{cc}{\left({\mathbf{\Sigma }}_{xx}-{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}{\mathbf{\Sigma }}_{yx}\right)}^{-1}& 0\\ 0& {\mathbf{\Sigma }}_{yy}^{-1}\end{array}\right]\\ & \times \left[\begin{array}{cc}1& -{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}\\ 0& 1\end{array}\right]\left(\left[\begin{array}{c}\mathbf{x}\\ \mathbf{y}\end{array}\right]-\left[\begin{array}{c}{\mathbf{\mu }}_x\\ {\mathbf{\mu }}_y\end{array}\right]\right)\\ & ={\left(\mathbf{x}-{\mathbf{\mu }}_x-{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}\left(\mathbf{y}-{\mathbf{\mu }}_y\right)\right)}^{\mathrm{T}}{\left({\mathbf{\Sigma }}_{xx}-{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}{\mathbf{\Sigma }}_{yx}\right)}^{-1}\\ & \times \left(\mathbf{x}-{\mathbf{\mu }}_x-{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}\left(\mathbf{y}-{\mathbf{\mu }}_y\right)\right)+{\left(\mathbf{y}-{\mathbf{\mu }}_y\right)}^{\mathrm{T}}{\mathbf{\Sigma }}_{yy}^{-1}\left(\mathbf{y}-{\mathbf{\mu }}_y\right)\end{array}$$

3.2.1 高斯推断的理解

从状态的先验分布（P(x)）出发，结合观测 值 $y_{\text{means}}$给出x的最佳估计，这个过程可以缩小x的方差，也就是让x的估计变好了。

3.3 高斯分布的线性变换

计算高斯分布的变换的关键就是计算均值和协方差，直接按照公式去推导即可。

3.4 高斯PDF的归一化积（一般PDF归一化积的高斯实现）：用于融合同一变量的复合高斯分布的多次观测

成立的前提是：
K 个⾼斯概率密度函数的归⼀化积（参考 2.1.6 节）仍然是⾼斯概率密度函数

其中融合后的均值方差如下

融合后的方差变小，意味着多次融合的估计值更好。

同理我们也可以对高斯分布做线性变换之后的高斯分布做归一化积

3.5 高斯分布的非线性变换

• 输入：高斯分布P(x), P(y|x)
• 输出：非高斯分布P(y)
• 假设：P(y|x)的噪声是零均值高斯噪声
  
  输出PDF是非高斯分布，这里使用线性化，在输出点用高斯分布近似