产量预测4 卡尔曼滤波

卡尔曼滤波 参考1
卡尔曼滤波 参考2
贝叶斯滤波
多元高斯分布

先给出终极公式：

${\hat{x}}_k^{\prime }$: 最优估计值
$P_k^{\prime }$: 最优估计的协方差
$K^{\prime }$: 卡尔曼增益(应该写在第一排)
${\hat{x}}_k$: 预测值
$P_k$: 预测值协方差
$H_k$: 转移矩阵
$\overrightarrow{z_k}$: 传感器观测值
$R_k$: 观测值高斯噪声 协方差

---

1 机器人工作引入

有一个依靠传感器和系统预测，从而在运作的小机器人，状态 $x_k$ 可以表示在 $k$ 时刻，机器人的位置与速度：
$$\begin{array}{r}x=\left[\begin{array}{c}p\\ v\end{array}\right]\end{array}\text{\hspace{1em}(1)}$$

假设速度不变，现对通过 $k-1$ 时刻对 $k$ 时刻机器人的运动进行预测：
$$\begin{array}{rlr}& p_k=p_{k-1}+& v_{k-1}\Delta t\\ & v_k=& v_{k-1}\end{array}\text{\hspace{1em}(2)}$$

2 矩阵表示

表达式可以修改成如下， ${\hat{x}}_k$ 为对 $k$ 时刻机器人的预测状态，$F_k$ 为状态转移矩阵：
$$\begin{array}{rl}{\hat{x}}_k& =\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]{\hat{x}}_{k-1}\\ & =F_k{\hat{x}}_{k-1}\end{array}\text{\hspace{1em}(3)}$$

求预测结果的协方差：
可获取到各变量的间的协方差如下， $P_k$ 为预测后 各变量的协方差：
因为：
$$\begin{gathered} Cov(x)=\Sigma \\ Cov(Ax)=A\Sigma A^T \end{gathered}$$
所以：
$$\begin{gathered} {\hat{x}}_k=F_k{\hat{x}}_{k-1} \\ {P}_k=F_k{P}_{k-1}{F}_k^T\text{\hspace{1em}(4)} \end{gathered}$$

3 加入控制变量

如果加入控制参数，就可加入机器人的加速度 $a$，
$$\begin{array}{rlrl}& p_k=p_{k-1}+& \Delta t{v}_{k-1}+& \frac{1}{2}a\Delta t^2\\ & v_k=& v_{k-1}+& a\Delta t\end{array}\text{\hspace{1em}(5)}$$

可以修改为两个矩阵相加来表示：
$$\begin{array}{rl}{\hat{x}}_k& =F_k{\hat{x}}_{k-1}+\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]a\\ & =F_k{\hat{x}}_{k-1}+B_k\overrightarrow{u_k}\end{array}\text{\hspace{1em}(6)}$$

$B_k$ 为控制矩阵， $\overrightarrow{u_k}$为控制向量

4 加入外部干扰

对于外部干扰，加入 $Q_k$ 高斯噪声处理，得到完整的预测表达式:
$$\begin{gathered} {\hat{x}}_k=F_k{\hat{x}}_{k-1}+B_k\overrightarrow{u_k} \\ {P}_k=F_k{P}_{k-1}{F}_k^T+Q_k\text{\hspace{1em}(7)} \end{gathered}$$

---

5 观测值 高斯分布

（看到这里脑袋宕机了，感觉有问题。待处理）
$z_k$为传感器观测值，$R_k$ 为观测噪声，观测值服从高斯分布：
$$N(x,{\mu }_1,{{\sigma }_1}^2)=N(x,\overrightarrow{z_k},R_k)\text{\hspace{1em}(8)}$$

6 预测值 高斯分布

（是预测值转换到观测值相同的单位或格式？）
一般情况下，传感器测量单位或指标不同于需要预测的单位，引入转换矩阵 $H_k$ 来表示预测值转换到与传感器值同单位的参数。所以，转换为相同单位，预测值的高斯分布为：
$$N(x,{\mu }_0,{{\sigma }_0}^2)=N(x,H_k{\hat{x}}_{k-1},H_k{P}_k{H}_k^T)\text{\hspace{1em}(9)}$$

7 卡尔曼增益 最优估计值

（how? why？）
有性质：高斯分布的乘积仍然是高斯分布
对$N(x,{\mu }_0,{\sigma }_0)\cdot N(x,{\mu }_1,{\sigma }_1)=N(x,{\mu }^{\prime },{\sigma }^{\prime })$，求新的$N(x,{\mu }^{\prime },{\sigma }^{\prime })$ 并归一化

${\mu }^{\prime }$ 取 ${\mu }_0,{\mu }_1$ 的加权平均：
$${\mu }^{\prime }={\mu }_0+\frac{{{\sigma }_0}^2({\mu }_1-{\mu }_0)}{{{\sigma }_0}^2+{{\sigma }_1}^2}\text{\hspace{1em}(10)}$$
${\sigma }^{\prime }$ 取 ${\sigma }_0,{\sigma }_1$ 的调和平均的二分之一：
$${{\sigma }^{\prime }}^2={{\sigma }_0}^2-\frac{{{\sigma }_0}^4}{{{\sigma }_0}^2+{{\sigma }_1}^2}\text{\hspace{1em}(11)}$$

令
$$k=\frac{{{\sigma }_0}^2}{{{\sigma }_0}^2+{{\sigma }_1}^2}\text{\hspace{1em}(12)}$$

那么
$$\begin{gathered} {\mu }^{\prime }={\mu }_0+k({\mu }_1-{\mu }_0) \\ {{\sigma }^{\prime }}^2={{\sigma }_0}^2-k{{\sigma }_0}^2\text{\hspace{1em}(13)} \end{gathered}$$

8 过程整合

(主要将式(12)，(13)整合到卡尔曼滤波当中)

---

待学习