状态估计问题-卡尔曼滤波

  思来想去，还是感觉有必要记录下卡尔曼滤波基础公式的推导，加深自己的理解，如有不准确的地方也欢迎大家评论指正！
  经典的卡尔曼滤波算法是Kalman在1960年提出的，此后出现了多种形式的变种，在机器人、自动驾驶和飞行控制等领域都有着广泛的应用。卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入观测数据，对系统状态进行最优估计的算法。通常它的输入信号中带有“过程噪声”和“观测噪声”，输出信号是对真实信号的最优估计，因此其过程可以看作是一个数据滤波的过程，目的是去除噪声以还原真实数据。

卡尔曼滤波-五大经典公式：

  这里还是引用网上卡尔曼滤波介绍文章中经典的工作流程图。不难看出，卡尔曼滤波器工作流程是一个不断迭代的过程，单个周期内分为预测（Predict）和更新（Update）两部分。这里引入几个关于系统状态量的概念：

• $x_k$：表示系统状态的真实值（通常无法直接得到）；
• $\overline{x_k}$：表示系统状态的预测值（通常根据上一时刻系统状态的估计值预测得到，也叫先验状态）；
• $\widehat{x_k}$：表示系统状态的估计值（通常是预测值加入观测信息后修正得到，也叫后验状态）；

预测：

• 输入：过去的最优状态$({\hat{x}}_{k-1},\widehat{P_k})$，外界对过程的影响$u_k$，环境的不确定度$Q_k$；

• 输出：预测的当前时刻系统状态$({\bar{x}}_k,\overline{P_k})$；

• 其他：对过程的描述转换$(F_k,B_k)$，与时间有关；
  $$\overline{x_k}=F_k{\hat{x}}_{k-1}+B_k{u}_k\text{\hspace{1em}(1)}$$ $$\overline{P_k}=F_k{\hat{P}}_{k-1}{F}_k^T+Q_k\text{\hspace{1em}(2)}$$
  更新：

• 输入：预测的当前时刻系统状态$({\bar{x}}_k,\overline{P_k})$，观测值的状态$(z_k,R_k)$，状态量到观测量维度的变换矩阵$H_k$；

• 输出：经过观测值修正后的最优估计状态$({\hat{x}}_k,\widehat{P_k})$；

$$K==\overline{P_k}{H}_k^T(H_k\overline{P_k}{H}_k^T+R_k{)}^{-1}$$ $$\widehat{x_k}=\overline{x_k}+K(z_k-H_k\overline{x_k})\text{\hspace{1em}(4)}$$ $$\widehat{P_k}=\overline{P_k}-K{H}_k\overline{P_k}=(I-K{H}_k)\overline{P_k}\text{\hspace{1em}(5)}$$

卡尔曼滤波-推导过程

  系统状态估计描述的是已知前一时刻的系统状态$x_{k-1}$和当前时刻的系统观测量$z_k$，如果获取当前时刻系统最可能的状态信息。通常需要构建状态方程和观测方程来进行系统状态的建模：
状态方程：
描述的是前一时刻的系统状态$x_{k-1}$与当前时刻系统状态$x_k$的关系；$$x_k=F_k{x}_{k-1}+B_k{u}_k+w_k$$其中：

• $F_k$为状态转移矩阵，$B_k{u}_k$为外部输入，$w_k$为过程噪声，一般满足高斯分布$w_k\sim N(0,Q_k)$。

测量方程：
  描述的是当前时刻的观测量$z_k$与当前时刻系统状态$x_k$的关系；$$z_k=H_k{x}_k+v_k$$其中：

• $H_k$为测量矩阵，$v_k$为测量噪声，一般满足高斯分布$v_k\sim N(0,R_k)$。

  我们都知道卡尔曼滤波是线性状态估计问题中最为直接的一种处理方法，那么如何理解卡尔曼滤波呢？可以从如下两个角度进行理解：一种是从概率分布的角度，一种是从最小化误差的角度。

1. 从概率分布的角度

  卡尔曼滤波将状态方程中的过程噪声假设成均值为0的高斯噪声，使得系统状态向量也可以看做是一个符合高斯分布的随机向量。同时对于测量方程中的测量噪声也假设成均值为0的高斯噪声，同样通过测量方程可以将系统状态向量转换到观测域，此时观测域的状态向量同样满足高斯分布。这样就可以得到两个关于系统状态向量的高斯分布（观测域下）：

1. 系统状态（预测值）满足$N(H_k{x}_k，H_k{P}_k{H}_k^T)$
2. 系统状态（观测值）满足$N(z_k，R_k)$

  其中预测的方差（多维称协方差）是根据系统状态$x_k$的协方差$P_k$及协方差传播定律（已知向量x 的协方差为$\Sigma$，协方差满足$Cov(Ax)=A\Sigma A^T$）得到的。
  概率论中提到两个高斯分布的联合概率分布依旧保持高斯特性，简单以下图为例，绿色和红色表示不同的高斯分布函数，蓝色为两个高斯分布的乘积，可以大概看出蓝色部分相较于原来的两个高斯分布，分布更为集中，形态上像是一个幅值被压缩的高斯分布。那么如果这个性质能够应用到前面提到的系统状态向量的两个高斯分布上，是不是就说明可以通过两个高斯分布的乘积来对系统状态进行更加可靠的估计呢？

两个高斯分布的乘积推导：

  假设两个高斯分布满足$N_1\sim ({\mu }_1,{\sigma }_1^2)$，$N_2\sim ({\mu }_2,{\sigma }_1^2)$，现在需要计算两者相乘后新的概率分布N：$$f(x)=\frac{1}{\sqrt{2\pi }\sigma }exp(-\frac{(x-\mu {)}^2}{2{\sigma }^2})$$ $$N(\mu ,{\sigma }^2)=N_1({\mu }_1,{\sigma }_1^2)\cdot N_2({\mu }_2,{\sigma }_2^2)$$ 将符号带入后，可得：
$$\begin{array}{rl}f(x)& =f(x_1)\cdot f(x_2)\\ & =\frac{1}{\sqrt{2\pi }{\sigma }_1}exp(-\frac{(x-{\mu }_1{)}^2}{2{\sigma }_1^2})\times \frac{1}{\sqrt{2\pi }{\sigma }_2}exp(-\frac{(x-{\mu }_2{)}^2}{2{\sigma }_2^2})\\ & =\frac{1}{2\pi {\sigma }_1{\sigma }_2}exp(-\frac{(x-{\mu }_1{)}^2}{2{\sigma }_1^2}-\frac{(x-{\mu }_2{)}^2}{2{\sigma }_2^2})\end{array}$$
单独取出$f(x)$的指数部分，即
$$\begin{array}{rl}\frac{(x-{\mu }_1{)}^2}{2{\sigma }_1^2}+\frac{(x-{\mu }_2{)}^2}{2{\sigma }_2^2}& =\frac{{\sigma }_2^2(x^2-2{\mu }_{1}x+{\mu }_1^2)+{\sigma }_1^2(x^2-2{\mu }_{2}x+{\mu }_2^2)}{2{\sigma }_1^2{\sigma }_2^2}\\ & =\frac{({\sigma }_1^2+{\sigma }_2^2)x^2-2x({\mu }_1{\sigma }_2^2+{\mu }_2{\sigma }_1^2)+({\mu }_1^2{\sigma }_2^2+{\mu }_2^2{\sigma }_1^2)}{2{\sigma }_1^2{\sigma }_2^2}\\ & =\frac{x^2-2x\frac{{\mu }_1{\sigma }_2^2+{\mu }_2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}+\frac{{\mu }_1^2{\sigma }_2^2+{\mu }_2^2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}}{\frac{2{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}}\\ & =\frac{(x-\frac{{\mu }_1{\sigma }_2^2+{\mu }_2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}{)}^2+\frac{{\mu }_1^2{\sigma }_2^2+{\mu }_2^2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}-(\frac{{\mu }_1^2{\sigma }_2^2+{\mu }_2^2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}{)}^2}{\frac{2{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}}\\ & =\frac{(x-\frac{{\mu }_1{\sigma }_2^2+{\mu }_2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}{)}^2}{\frac{2{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}}+\frac{\frac{{\mu }_1^2{\sigma }_2^2+{\mu }_2^2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}-(\frac{{\mu }_1^2{\sigma }_2^2+{\mu }_2^2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}{)}^2}{\frac{2{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}}\end{array}$$
前面已经变成了$\frac{(x-\mu {)}^2}{2{\sigma }^2}$的形式，其中$\mu =\frac{{\mu }_1{\sigma }_2^2+{\mu }_2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2},{\sigma }^2=\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}$。继续对（1-2）式后续的常数部分进行简化，即
$$\begin{array}{rl}\frac{\frac{{\mu }_1^2{\sigma }_2^2+{\mu }_2^2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}-(\frac{{\mu }_1^2{\sigma }_2^2+{\mu }_2^2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}{)}^2}{\frac{2{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}}& =\frac{({\mu }_1^2{\sigma }_2^2+{\mu }_2^2{\sigma }_1^2)({\sigma }_1^2+{\sigma }_2^2)-({\mu }_1^2{\sigma }_2^2+{\mu }_2^2{\sigma }_1^2{)}^2}{2{\sigma }_1^2{\sigma }_2^2({\sigma }_1^2+{\sigma }_2^2)}\\ & =\frac{({\mu }_1-{\mu }_2{)}^2}{2({\sigma }_1^2+{\sigma }_2^2)}\end{array}$$
因此，两个高斯概率密度函数的乘积可以写成

其中：
$$\mu =\frac{{\mu }_1{\sigma }_2^2+{\mu }_2{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}={\mu }_1+\frac{{\sigma }_1^2({\mu }_2-{\mu }_1)}{{\sigma }_1^2+{\sigma }_2^2}$$ $${\sigma }^2=\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}={\sigma }_1^2-\frac{{\sigma }_1^4}{{\sigma }_1^2+{\sigma }_2^2}$$因此两个高斯分布的乘积仍为高斯分布，且均值为μ，方差为${\sigma }^2$，$S_g$被称为缩放因子，表示相乘后的高斯分布函数相比于标准高斯分布是一个被压缩或者放大的高斯分布。为了方便表达，我们令$K=\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}$，这样两个高斯分布相乘后新的高斯分布的均值和方差可以表示为： $$\mu ={\mu }_1+K({\mu }_2-{\mu }_1)$$ $${\sigma }^2={\sigma }_1^2-K{\sigma }_1^2$$ 将上式拓展到多维空间，同样适用，即：$$K={\Sigma }_1({\Sigma }_1+{\Sigma }_2{)}^{-1}$$ $$\mu ={\mu }_1+K({\mu }_2-{\mu }_1)$$ $$\Sigma ={\Sigma }_1-K{\Sigma }_0$$

状态估计方程代入：

  前面提到现在有两个关于系统状态向量的高斯分布（观测域下）：

1. 系统状态（预测值）满足$N(H_k{x}_k，H_k{P}_k{H}_k^T)$
2. 系统状态（观测值）满足$N(z_k，R_k)$

则两个高斯分布相乘结果得到的新分布满足：
$$K^{\prime }=H_k{P}_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1}$$ $${\mu }^{\prime }=H_k{x}_k+K^{\prime }(z_k-H_k{x}_k)$$ $${\Sigma }^{\prime }=H_k{P}_k{H}_k^T-K{H}_k{P}_k{H}_k^T$$
  进一步将观测域的系统状态量通过变化状态域下，即等式两边同时乘上 $H_k^{-1}$，可以得到$$K=P_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1}$$ $$\mu =x_k+K(z_k-H_k{x}_k)$$ $$\Sigma =P_k-K{H}_k{P}_k=(I-K{H}_k)P_k$$   此时，新的$(\mu ，\Sigma )$就表示联合状态量和观测量信息后得到的系统状态估计及协方差。其中 K 表示卡尔曼增益，决定了观测值与预测值在融合过程中的权重。

2. 从最小化误差的角度

  前面提到系统状态量根据隐含信息的不同，可以分为$x_k$(状态的真实值)，$\overline{x_k}$(状态的预测值)，$\widehat{x_k}$(状态的估计值)。预测值和估计值的区别在于：

1. $\overline{x_k}$(状态的预测值) 仅仅是根据上一时刻状态结合经验推测出的一个结果，从概率上讲是一个先验结果；
2. $\widehat{x_k}$(状态的估计值) 是带有观测信息的，从概率上讲是一个后验结果。

对于两者和真实值之间的误差，可分别表示为：

先验误差：指真实值与预测值之间的误差，即$$\overline{e_k}=x_k-\overline{x_k}$$后验误差：指真实值与估计值之间的误差，即$$\widehat{e_k}=x_k-\widehat{x_k}$$   而卡尔曼滤波算法，就是依据均方误差MSE准则，使得后验误差最小。首先，定义先验误差和后验误差的协方差矩阵为 $\overline{P_k}$和 $\widehat{P_k}$，则有$$\overline{P_k}=E[\overline{e_k}\cdot {\overline{e_k}}^T]$$$$\widehat{P_k}=E[\widehat{e_k}\cdot {\widehat{e_k}}^T]$$
  推导过程如下（因懒…这里以手推记录上传）如果觉得影响阅读，可以移步到参考链接查看其他博主推导过程：

总结：

卡尔曼滤波器的工作过程文章最开始也已经描述过了，这里就简单在介绍下里面涉及到的各种变量吧。

• $(\widehat{x_{k-1}},\widehat{P_{k-1}})$表示 k-1 时刻系统的状态估计值及其协方差。（一般理解是前一时刻的后验估计值）；
• $(\overline{x_k},\overline{P_k})$表示 k 时刻系统的状态预测值及其协方差。（当前时刻的先验估计值）；
• $(\widehat{x_k},\widehat{P_k})$表示 k 时刻系统的状态估计值及其协方差。（当前时刻的后验估计值）；
• $z_k$为 k 时刻的观测量；
• $F_k$为状态转移矩阵，$B_k{u}_k$为外部输入，$w_k$为过程噪声，一般满足高斯分布$w_k\sim N(0,Q_k)$；
• $H_k$为测量矩阵，$v_k$为测量噪声，一般满足高斯分布$v_k\sim N(0,R_k)$。

再次强调，一般的卡尔曼滤波是基于以下条件假设的：

1. 状态方程和测量方程是线性的！！！
2. 过程噪声$w_k$和测量噪声$v_k$，必须是高斯白噪声，及满足$w_k\sim N(0,Q_k)$和$v_k\sim N(0,R_k)$。
3. 如果是非线性系统，请参考其他形式的卡尔曼滤波，如EKF、UKP等；
   

参考资料：

1. 卡尔曼滤波基本公式推导（高斯乘积法）_卡尔曼滤波公式推导-CSDN博客
2. 卡尔曼滤波器–基础知识及公式推导
3. 深入浅出理解卡尔曼滤波【实例、公式、代码和图】
4. 卡尔曼增益推导-基于最小化误差思想