状态估计算法

---

前言

  介绍一些常用的状态估计算法，重点介绍卡尔曼滤波的原理及实现，本文需要一定高等数学、线性代数与概率论的基础

---

一、贝叶斯滤波

  贝叶斯滤波（Bayesian filtering） 是一种概率推断的方法，用于估计随时间推移的系统状态。它基于贝叶斯定理和递归贝叶斯估计的概念，通过使用先验概率和观测数据，更新状态的后验概率。
  贝叶斯滤波通常用于在有噪声或不完全观测的情况下，进行对系统状态估计。在如目标跟踪、机器人定位和导航、语音识别等领域有着广泛应用。贝叶斯滤波比较适用于有限状态空间的问题

1. 对于有限状态空间的问题，贝叶斯滤波是一种非常有效的方法，并且相对容易转换为代码实现。由于状态空间是有限的，我们可以使用离散的概率分布或概率矩阵来表示状态和观测之间的转移概率，其应用方式有离散贝叶斯滤波、静态二值贝叶斯滤波和粒子滤波器。例：判断门是开的还是关的，其状态空间只有两种情况，开和关。
2. 对于无限状态空间问题，则不适合用贝叶斯来估计，在无限状态空间中，由于状态的数量是无穷的。对对上一时刻的所有状态进行积分是办不到的事情，通常会考虑使用其他类型的滤波器或估计方法，如KF、EKF、UKF、IF、EIF这类高斯滤波器。例：判断某一个变量的值，这个值可以是任意的数，其状态空间就是无限的。

  常见的贝叶斯滤波算法包括卡尔曼滤波（Kalman Filter）和粒子滤波（Particle Filter）。卡尔曼滤波适用于线性系统和高斯噪声，而粒子滤波则适用于非线性系统和非高斯噪声。

二、卡尔曼滤波

2.1 KF简介

  卡尔曼滤波（Kalman Filter） 是一种在不确定状况下组合多源信息得到所需状态最优估计的一种方法，你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。

  卡尔曼滤波属于是基于贝叶斯最大后验估计的推论，它假设系统的状态与观测误差都是高斯分布，并且系统的动态模型和测量模型均为线性的。在卡尔曼滤波中预测和更新步骤的计算，可以通过线性方程和协方差矩阵的更新来实现，从而大大简化了计算过程，并且内存占用量低，推理速度快，比较适合资源受限制的场景。卡尔曼滤波算法分为两步:

1. 预测：根据上一时刻(k-1时刻) 的后验估计值来估计当前时刻(k时刻) 的状态，得到k时刻的先验估计值;

2. 更新：使用当前时刻的测量值来更正预测阶段估计值，得到当前时刻的后验估计值。

2.2 基本线性模型

  卡尔曼滤波是一种高效率的递归滤波器，其应用基于以下三个假设前提：

• 当前时刻状态只和上一时刻状态有关
• 模型和系统均满足线性关系
• 引入的噪声符合高斯分布

  基于上述假设，我们可以得到一离散线性动态系统的模型如下所示：
$$x_k=F{x}_{k-1}+B{u}_{k-1}+w_k\text{\hspace{1em}(1-1)}$$
$$z_k=H{x}_k+v_k\text{\hspace{1em}(1-2)}$$

  式1-1为系统状态方程，式1-2为系统观测方程，其参数如下：

• $x_k$：表示k时刻的真实值，是待估计的值，例如位置、速度
• $x_{k-1}$：表示k-1时刻的真实值，即上一时刻的真实值
• $u_{k-1}$：表示k-1时刻的控制输入量
• $w_k$：表示过程噪声
• $z_k$：表示k时刻的观测值，即测量值
• $v_k$：表示测量噪声
• $F$：表示状态转移矩阵
• $B$：表示控制矩阵
• $H$：表示观测转移矩阵

  由于噪声符合高斯分布，设均值为0，协方差矩阵分别为Q和R，则有$p(w_k)\sim N(0,Q)$ 和 $p(v_k)\sim N(0,R)$，关于协方差矩阵下面2.3.2小节介绍

2.3 KF公式推导

2.3.1 预测值

  为了更好的理解上面模型，我们假设状态值$x$为矢量，这里以运动模型的速度$v$和位置$p$为例，则有
$$x=\left[\begin{array}{c}p\\ v\end{array}\right]$$
  当然$x$并不一定为二维数据，比如在温度系统是一维，只有温度参数，也可以在三维系统。此时再假设加速度为$a$，根据速度位置计算公式有：
$$\begin{gathered} v_t=v_{t-1}+a\Delta t \\ {p}_t=p_{t-1}+v_{t-1}\Delta t+\frac{1}{2}a(\Delta t{)}^2 \end{gathered}$$

  则上式1-1可以变为
$$\left[\begin{array}{c}{p}_t\\ v_t\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{p}_{t-1}\\ v_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{(\Delta t{)}^2}{2}\\ \Delta t\end{array}\right]a_{t-1}+w_t$$
  此时有$F=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]$，$B=\left[\begin{array}{c}\frac{(\Delta t{)}^2}{2}\\ \Delta t\end{array}\right]$

  同理对于式1-2，由于线性关系，我们假设测量与实际值关系为1:1，由于符号冲突，这里设观测误差为$c$，可变为
$$\left[\begin{array}{c}{z}_t(p)\\ z_t(v)\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{p}_t\\ v_t\end{array}\right]+c_t$$
  此时有$H=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$

  在实际情况中，过程噪声$w_k$与测量噪声$v_k$是未知的，所以$x_k$充满了不确定性。此时再回到卡尔曼滤波的递归特性，它可以不停的修正观测值。由于测量值包含噪声，此时我们就可以忽略噪声，而将其融入到转移矩阵中

  我们使用上一时刻的最优估计值${\tilde{x}}_{k-1}$来替代真实值，并将${\bar{x}}_k$称为当前时刻通过过程模型得到的预测值，也叫做先验估计值，此时得到我们最终想要的公式：
$${\bar{x}}_k=F_k{\tilde{x}}_{k-1}+B_k{u}_{k-1}\text{\hspace{1em}(2-1)}$$

  同理观测值的预测有
$$z_k=H_k{\tilde{x}}_k$$

2.3.2 先验误差协方差矩阵

  首先定义两个变量：我们把真实值和预测值的差叫做先验误差${\bar{e}}_k$，并且把真实值和估计值的差叫做后验误差${\tilde{e}}_k$，即
$$\begin{gathered} {\bar{e}}_k=x_k-{\bar{x}}_k \\ {\tilde{e}}_k=x_k-{\tilde{x}}_k \end{gathered}$$
  除此之外，我们继续使用上一小节的速度位置模型来帮助理解。卡尔曼滤波假设两个变量（位置和速度在这个例子中）都是随机的，并且服从高斯分布。那么我们可以引入协方差$\sigma$，来表示速度$v$与位置$p$之间相关程度，记为
$$\sigma =Cov(p,v)$$
  再将协方差变成协方差矩阵$\Sigma$有
$$Cov(x)=\Sigma =\left[\begin{array}{cc}{\sigma }_{pp}& {\sigma }_{pv}\\ {\sigma }_{vp}& {\sigma }_{vv}\end{array}\right]$$
  由于${\sigma }_{pv}={\sigma }_{vp}$，很明显有$\Sigma ={\Sigma }^T$。将式1-1与式2-1代入到先验误差${\bar{e}}_k$里有
$$\begin{array}{rl}{\bar{e}}_k& =x_k-{\bar{x}}_t\\ & =F{x}_{k-1}+B{u}_{k-1}+w_k-F{\tilde{x}}_{k-1}-B{u}_{k-1}\\ & =F(x_{k-1}-{\tilde{x}}_{k-1})+w_k\\ & =F{\tilde{e}}_{k-1}+w_k\end{array}$$
  恰好得到先验误差${\bar{e}}_k$与后验误差${\tilde{e}}_k$的关系，再将上式代入到公式$Cov(Ax)=A\Sigma A^T$，由于$w_k$的协方差矩阵为Q（2.2小节有设），此时我们假设$e_k$的协方差矩阵为P，那么有
$$\begin{array}{rl}{P}_k& =Cov({\bar{e}}_k)\\ & =Cov(F{\tilde{e}}_{k-1}+w_k)\\ & =Cov(F{\tilde{e}}_{k-1})+Cov(w_k)\\ & =F{P}_{k-1}{F}^T+Q\end{array}$$
  此时得到先验误差协方差矩阵${\bar{P}}_k$公式
$${\bar{P}}_k=F_k{P}_{k-1}{F}_k^T+Q_k\text{\hspace{1em}(2-2)}$$

2.3.3 卡尔曼增益

  先介绍两种误差：估计误差$e_{est}$和测量误差$e_{mea}$，假设卡尔曼增益用$K$来标示，则有
$$K_k=\frac{e_{es{t}_{k-1}}}{e_{es{t}_{k-1}}+e_{me{a}_{k-1}}}$$
  这里我们用一维高斯分布来帮助理解，每个变量都有一个均值$\mu$，表示随机分布的中心（最可能的状态），以及方差${\sigma }^2$，表示不确定性，有
$$N(x,\mu ,\sigma )=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}$$
  假设预测值符合分布$N(x,{\mu }_0,{\sigma }_0)$，测量值符合分布$N(x,{\mu }_1,{\sigma }_1)$，那么最优估计值就是两个分布相融合的地方

  这里我们直接将两个分布函数相乘，即$N(x,{\mu }_0,{\sigma }_0)\cdot N(x,{\mu }_1,{\sigma }_1)$，可以得到分布$N(x,{\mu }^{\prime },{\sigma }^{\prime })$，有
$${\mu }^{\prime }={\mu }_0+\frac{{\sigma }_0^2({\mu }_1-{\mu }_{}}{{\sigma }_0^2+{\sigma }_1^2}$$