1. Why
为什么用概率来描述？因为机器人的环境是不确定的，所使用的传感器也是有误差的
- 某个随机变量的期望：$E [X]=\sum_{i=1}^{n} p_{i} x_{i}$
- 为什么只使用单峰模型？因为机器人在某一时间的位置是固定的，并不会出现在不同位置
2. 高斯
中心极限定理：样本数够多，就会出现正态分布
使用单峰分布处理起来非常困难。
状态的更新（两个正态分布的相乘）可以用贝叶斯来计算
$$
\begin{array}{c}
\mu_{\text {new }}=\frac{\mu_{1} \sigma_{2}^{2}+\mu_{2} \sigma_{1}^{2}}{\sigma_{1}^{2}+\sigma_{2}^{2}} \
\sigma_{\text {new }}=\frac{\sigma_{1}^{2} \sigma_{2}^{2}}{\sigma_{1}^{2}+\sigma_{2}^{2}}
\end{array}
$$
有的时候状态更新只是两个正态分布的相加，那么新的期望和方差就是两个期望和方差的简单相加
3. 概率生成定理
$z$表示测量，$u$表示指令，$x$表示状态
第一定理：$p\left(x_{t} \mid x_{0: t-1}, z_{1: t-1}, u_{1: t}\right)=p\left(x_{t} \mid x_{t-1}, u_{t}\right)$
- 也就是说，当前状态只和上一时刻状态以及此刻的指令有关
第二定理：$p\left(z_{t} \mid x_{0: t}, z_{1: t-1}, u_{1: t}\right)=p\left(z_{t} \mid x_{t}\right)$
- 如果$x_t$是完整的，那么当前的测量概率只和当前的状态有关
4. 贝叶斯法则
$$
后验概率=\frac{可能性先验概率}{边界概率}=\frac{观测上一时刻概率}{1}
$$
利用贝叶斯法则估算状态：
- 预测步骤：计算先验概率，不考虑观测，只考虑指令会带来的变化：利用模型
- 这一步会引入误差，包括传感器、执行器上的
- 矫正步骤：利用上一步计算出的先验概率，加上观测，计算后验概率（贝叶斯法则）
5. 卡尔曼滤波
两个重要假设：
- 线性
- 高斯
主体框架和贝叶斯估计是一样的，最大的区别是数学表达：
- 卡尔曼滤波使用了高斯概率
也就是说在卡尔曼滤波中，无论是先验还是后验抑或是置信，都是一个高斯分布
$$
\mu_{\text {new }}=\frac{\mu_{1} \sigma_{2}^{2}+\mu_{2} \sigma_{1}^{2}}{\sigma_{1}^{2}+\sigma_{2}^{2}}
$$
回想我们之前是如何计算新的期望的，显然可以拆成两个
$$
\mu=W_1\mu_1+W_2\mu_2
$$
我们是根据两个的权重线性叠加的。且$W_1+W_2=1$，因此我们引入新的变量$K$——卡尔曼增益
卡尔曼增益在卡尔曼滤波中起着重要的作用。令$K=W_1$
则有：
$$
\begin{aligned}
\mu &=K \mu_{z}+(1-K) \bar{\mu} \
&=\bar{\mu}+K\left(\mu_{z}-\bar{\mu}\right)
\end{aligned}
$$
和
$$
\sigma^2=K\sigma_{z}^{2}
$$
6. 卡尔曼滤波框架
预测部分。执行器模型一般是线性的，在目前的状态下直接相加一些变量，期望和方差都是相加
$$
\bar x=Fx+Bu\
\bar P=FPF^T+Q
$$
其中x是状态，P也是状态是方差（也可以理解成误差），F是状态转移矩阵，Q是过程协方差，也可以理解为在行动过程中引入的噪声，B是输入矩阵，u是系统的控制输入矫正部分。利用传感器观测到并使用贝叶斯估计计算新的状态
$$
y=z-H\bar z\
K=\bar P H^T(H\bar P H^T+R)^{-1}\
x=\bar x+Ky\
P=(I-KH)\bar P
$$
其中y是误差，K是卡尔曼增益，x和P是新的状态
7. 卡尔曼增益
卡尔曼增益的更新
$$
K=\bar P H^T(H\bar P H^T+R)^{-1}
$$
- R是测量噪声，由传感器的制造商给出
- $(H\bar P H^T+R)$总的噪声，是预测噪声+测量噪声
- 为什么K如此复杂？因为矩阵的除法是没有定义的，因此计算总的噪声再取逆