从状态观测器角度理解卡尔曼滤波

   本文假定读者已学习过现代控制原理与概率论的相关知识，约定如内容涉及该学科则自动跳过解释，文章基于作者对卡尔曼滤波器的认识，融入了一些个人想法，并不完全按照卡尔曼滤波的原文进行公式推理，如有错误，欢迎指出。

卡尔曼滤波解决了什么问题？

   卡尔曼滤波是经典的预测算法，它能在存在噪声和干扰的情况下对系统的状态进行最优估计。通信与控制中一类重要的理论和实践问题是统计性质的。这些问题包括：(1)随机信号的预测；(2)将随机信号与随机噪声分离；(3)在存在随机噪声的情况下检测已知形式的信号(脉冲、正弦)。如果根据原理用通俗的话来讲，就是利用$t=k$时刻系统的输出值和观测器的输出值，预测系统在 $ t=k+1 $时刻的输出值。一个常见的应用是利用卡尔曼滤波进行多目标跟踪。

理论公式推导

   我们将主要讨论离散（或采样）动态系统；换句话说，信号将在等间隔的时间点（采样瞬间）观察到。通过适当选择时间尺度，可以选择连续采样瞬间（采样周期）之间的恒定间隔作为单位。因此，与时间有关的变量将始终是整数。对离散动态系统的限制根本不是必要的（至少从工程角度来看）；然而，通过使用离散性，我们可以保持数学的严谨性和基础性。下面给出该离散动态系统的运动方程与观测方程：

   我们假设了马尔可夫性，即当前时刻的状态只和上一个时刻有关，并假设过程噪声${\omega }_k$和观测噪声$v_k$服从均值为$0$的高斯分布：

$R$和$Q$为两个噪声的协方差矩阵，输入、输出和噪声之间两两独立。然后我们构造该系统的观测器

   可以看到笔者构造的观测器对过程噪声和观测噪声是未知的。按照控制理论的观点，开环观测器随着时间的推移它的输出值将会越来越不准确，一个经典的解决办法则是给观测器添加一个输出反馈通道，使得观测器能够逼近系统的真实状态，设附加的反馈矩阵为$K$。从闭环观测器系统框图可以看到，笔者将系统输出和开环观测器输出之间的误差作为反馈信号传回给观测器的反馈通道，这里稍微与我们认识的观测器有些许不同。我们将加入反馈后的状态值称为状态的反馈估计：

   在某些博文或者论文中会叫这个反馈估计为最优估计，但是为了保证整篇文章在状态观测器的解释方法不受其他解释的束缚，这里是笔者自行定义的名称。
   接下来，为了保证观测器的反馈估计$\widetilde{x_k}$能够更好地趋近原系统的状态$x_k$，我们需要定义两个误差项，一个是系统状态和反馈估计之间的误差$e_k$和系统状态与开环观测器状态之间的误差$\widehat{e_k}$，具体定义如下：

   两个误差项之间是有关联的，怎么知道他们之间的关系？将公式(3)的等号两边取负号再加上$x_k$，则可得：

   由于误差项已经带有噪声，从概率的角度考虑，我们应该保证反馈估计误差的方差尽可能地小，这样就能使我们对观测器的预测准确度更大：

   因为$P_k$是$e_k$的方差矩阵，所以$P_k$对角线上的元素为$e_k$每个元素的方差，故我们的目标函数可以设定为$P_k$的迹：

   我们的目标为设计$K$，使得目标函数J能够取到最小值，根据公式(7)求$tr(P_k)$关于$K$的导数并让它等于$0$，使得它能取到极小值，通过极小值之间比较可以得到最小值：

   上面应用矩阵求偏导的公式$(\partial tr(AB))/\partial A=(\partial tr(BA))/\partial A=B^T$，这样就得到了K：

   当$K$满足公式(10)时，反馈估计误差$e_k$的方差取到最小值。可以看到$K$每时每刻都在变化，并不像传统的闭环观测器将$K$设计为一个定值。将公式(10)代入到公式(7)中，得到更简洁的$P_k$迭代公式：

   在得到$K$以后，根据公式(3)我们就得到了反馈估计$\widetilde{x_k}$,我们用这个反馈估计$\widetilde{x_k}$作为新的状态量输入到观测器中进行对原系统的下一时刻的估计，根据公式(1.2)：

   但是此时我们不知道下一时刻的观测器误差的方差$\widehat{P_{k+1}}$，因为这关系到下一时刻K的更新。由于$\widehat{P_{k+1}}=D(\widehat{e_{k+1}})$从公式(4)也就$\widehat{e_{k+1}}$的定义，和公式(1.1)与公式(12)也就是系统和观测器的运动可以得到：

   则观察公式(6.1)，有：

   此时整个卡尔曼滤波的迭代过程需要的公式已经全部推理完，所需要的为公式(12)、(14)、(10)、(3)和(11)，整理一下算法的迭代步骤如下：
1)预测：

2)更新：

3)计算反馈估计和其误差方差：

   简单说一下卡尔曼滤波算法的流程，首先是利用上一时刻的反馈估计$\widetilde{x_k}$作为观测器的新的状态变量进行计算，得到了观测器对下一时刻的预测值$\widetilde{x_{k+1}}$,然后更新下一时刻的反馈矩阵$K$，并利用这个更新后的反馈矩阵和系统下一时刻的输出值$y_{k+1}$得到下一时刻的$\widetilde{x_{k+1}}$ ,再回到1)进行预测。