卡尔曼滤波器学习笔记（二）：随机过程和线性卡尔曼滤波器

随机过程的模型建立

一般地，我们研究的随机过程是一个动态的系统(Dynamical Systems).

其中，是系统中设置的参数，是状态向量，加入时间作为参数代表状态向量的取值随时间变化。

为了简化我们对系统的建模，首先把时间和系统参数对系统迭代的影响忽略掉，简化上面的等式：

()是连续系统的描述，更常见的是离散系统，我们把时间离散化，得到：

上面的等式仍然不是很理想，因为经常性的，是一个非线性的函数，对计算机来说，很难迭代地处理这样的函数，而且我们也很难给出准确的函数表达式。

那么，需要对()进行改写，在改写的过程中，考虑控制量的引入,如果系统建模方程中有高阶的导数，可以通过定义新的高阶导数作为变量组成维数更多的状态向量来降低阶数，最终得到个一阶的微分方程。这里具体怎么做就不展开了，并不属于这篇文章的重点内容，我们先直接使用结论。

()描述的是系统在当前状态下，下一时刻会如何改变，是一个一阶的微分方程组（因为状态向量是多维度的），但是这样的方程组也不是我们想要的，我们期望是可以根据前一时刻的状态直接线性运算得到后一时刻的预测状态。

状态转移方程

() 描述的是下一时刻系统状态的增长或者是变化和当前的状态有什么关系，假如没有加入人为的控制，比如，让一个倒立摆自由运动，不提供平衡的直线运动单元，那么这个系统的一些特性，比如可控性，稳定性都可以通过研究的特征值和特征向量来快速得到。 加入人为的控制量可以改变系统的特性，按照我们的期望来改造，Brunton老师说得是manipulate。在动态系统的控制领域，对矩阵的研究会更多，的学术用语为动态系统矩阵（System Dynamics Matrix），在状态估计领域，我们对状态转移方程更关心。实际系统经常是非线性的，但是在局部我们可以线性化。目前，从一阶微分方程组如何推导出状态转移方程是本次不讨论的内容，但是是有若干种技术去实现。

()中的叫做状态转移矩阵（State Transition Matrix）。

非线性的系统状态转移矩阵是根据系统的雅可比矩阵来确定的，每次迭代都会变。后续需要确认是不是这样。

到目前为止，根据系统的物理定律我们在没有测量的状态下得到系统状态是如何迭代的，但是需要注意()中的：系统噪声。随着时间迭代，我们仅仅使用状态转移方程估计系统状态会越来越不准，因为描述状态的协方差矩阵会变大。

随机过程的贝叶斯后验概率分布推导

根据第一讲贝叶斯滤波的基本思路，系统迭代的过程中，需要有测量数据做融合，从概率论角度讲就是测量数据作为随机变量，根据传感器的特性，它的似然概率分布是已知的：，那么在预测步骤之后，有状态向量的先验分布，那么我们的目标是得到基于两者的后验分布。

在推导之前，需要非常明确这几条：

1. 状态向量迭代的过程虽然是离散的，例如, 时刻的状态，但是在某一时刻的取值仍然是连续的随机变量，我们在第一篇里面得到的结论仍然适用于某一时刻的情况。
2. 在不同的时刻下，先验、后验分布都在变化，先验分布变化是因为系统因为物理、化学等规律随之前的状态产生了新的变化 -- 就是由状态转移方程描述的。

后面的公式当中，表示在时刻也就初始时刻的概率密度函数，是第一次迭代之后的先验概率分布函数，是融合了似然概率之后的后验概率分布函数。

状态转移的概率分布推导

第一个需要推导的是如何根据和()推导 重写():

()可以理解成对随机变量进行逻辑运算，后面会使用到。

对进行全概率公式展开：

到这里很容易迷茫不知道怎么办，看这个条件概率，和肯定是相关的，因为有线性变换，控制量的输入和随机的噪声输入，相关性是因为第一项还是部分项还是全部呢？ 这时就需要()。

令，()带入():

继续推导得到：

这里，条件概率变为了当取某一数值时，保持在的条件概率。 因为我们假设和相互独立，根据独立性，得到：

()代入()得到：

观察() 只存在于当中，并且这个概率可以改写为的概率分布，相当于在概率密度曲线和坐标横轴围成的区域上截取了很短的一段。所以一般性地，就可以将求和，取极限和微小区域的概率用积分的形式来描述：

概率分布函数是概率密度函数的原函数，根据微积分第一基本定理，对求导得到：

当时，上面的推导仍然成立，需要特殊提出来的是和相互独立。

测量更新的概率分布推导

这里首先需要给定测量出来的向量作为随机变量和系统状态之间具有如下的关系：

是测量噪声，也当作随机变量处理。

再次列出来连续随机变量的贝叶斯公式：

测量向量是不同的随机变量，它们的分布是不同的。

后验概率分布如下：

需要利用()对()中的似然概率和边缘概率进行推导：

首先是似然概率：

条件概率当中的条件可以拿来参与计算，()变为：

条件概率的条件和取值相互独立，所以去掉条件：

然后是边缘概率。

到这里，我最开始对于为什么需要把分母的那个边缘概率展开为全概率是不理解的，它其实就是一个不为零的常量，直接放在那里不是就可以了么？

在 Kalman and Bayes Filters in Python3.12 章节讲到了这个量叫做evidence，就是不考虑当前状态在哪里的情况下，测量值出现的概率。看到这里，再仔细想一想，这样的一个概率其实是很没有根据的，还不如把所有的测量可能和在当前可能下的条件概率做全概率积分更有意义。所以有了下面的推导。

那么问题又来了，我们怎么寻找那个全概率的“基底”呢？就是上一篇文章中说的的样本到底是怎么样的一个概率分布呢？自然地，那个初始的的先验分布经过了一个时间周期其实已经过时了，并不能很好地反映最新的情况，那么经过状态转移方程得到的先验概率分布更符合当前状态分布，那么可以利用这个分布和条件概率的乘积积分来计算：

令

得到：

因为在实际计算的过程中，预测步骤得到的的新分布和预测之前其实是不一样的，并且测量更新之后的分布也发生了变化，所以把第次预测之后测量更新之前的分布写成，把测量更新之后的分布写成得到最终的随机过程贝叶斯迭代公式：

卡尔曼滤波器

引入期望的几何意义

()、()和()中，每次迭代需要知道概率密度，知道了概率密度之后，还需要根据概率密度确定一个“最佳”数值，比如确定在期望的位置为最佳。这样实际工程中没办法使用，尤其是实时性很强的系统，每秒需要处理上百次数据，每次积分不现实。

Kalman在1960年发表的论文当中，首先论证了如何最小化估计和真实值之差在给定测量值条件下的期望，解就是状态在测量条件下分布的期望。

()是随机变量的期望。

论文中还讲了正交投影，把测量空间内的随机变量分解为若干个单位正交的随机变量的线性组合，定义随机变量正交需要满足乘法期望为零。所以，任意随机变量可以分解为两部分：一部分属于测量空间，一部分属于正交于测量空间。

测量空间的表示

，可以由如下表达式得到：

()中的期望部分就是对随机变量向做投影得到的系数。

正交空间的表示

论文中证明了在正交空间内的任意随机变量，都和测量空间内的随机变量正交，下面的符号代表了正交空间内的随机变量：

状态变量的最优估计就是向测量空间做正交投影得到的新的随机变量

论文中证明了这一点。在阅读论文的过程中，我发现，推导过程和模型很类似于最小二乘法在向量空间中的推导，所以下面的描述是对照最小二乘和随机变量的最优估计来进行。

在我之前的一篇文章《投影矩阵和最小二乘》中，已知空间当中的一些点，如何找到一条直线使得这条直线到每一个已知点的距离最小，这个问题和现在的随机变量估计问题有很深的类似关系。

1. 给定的已知点就是这里的测量，而且不同时刻的测量这个随机变量才对应于最小二乘里面的点
2. 最小化一个目标函数，在最小二乘当中，最小化的是误差绝对值的求和，在这里最小化的是估计出来的随机变量和真实值的差的损失函数的期望，文中提到最常见的损失函数是二次方的
3. 最小二乘当中，为了求出那条直线，需要确定两个参数和，在二维空间正好参数的确定和描述一个点所需要的维度相同，矩阵的列向量所形成的空间对应的是这里测量空间
4. 如果随机变量、期望都是0，这里最小化损失函数的期望所对应的最优估计(Optimal Estimation)就是当前的随机变量向测量空间做投影得到，对应于最小二乘中使用投影矩阵对向量左乘得到投影向量
5. 得到了投影之后的随机变量通过()就可以得到对应的参数。

论文当中的演进思路

1. Optimal Estimates: 给定了一组测量值，如何最优估计出来带有噪声的测量值所测量的状态向量？Wiener指出状态估计问题就是需要使用概率理论和统计的方法来解决。论文当中的定理1，说的是对状态向量的最优估计（最优的意义是把估计量和实际量做差当作一个随机变量，然后对该变量求loss 一般是）等价于状态向量在出现测量的条件下的期望。就是说，首先，我们需要搞清楚这个条件概率的分布，其次，把这个分布的期望当作最终的估计量--符合我们对事情的认知，因为我们经常通过求平均数来获取一个数据更加准确的估计。求期望，其实就是统计平均。

2. Orthognal Projection: 讲的是给定一组测量值且这些测量向量的元素一一对应状态向量中的元素，且和是期望为0的正态分布随机变量，在这些测量的条件下的最优估计是状态正交投影到测量空间得到的投影向量。但是到目前，我们仅仅是知道投影向量一定是测量值的线性组合（因为对那些basis-- 做投影得到的系数和对具体的测量值做投影得到系数，虽然具体的系数不一样，但是组合起来都代表同一个向量，具有等效性，而且测量值是直接得到的数据，为了方便，那么就直接使用测量值的线性组合来取代basis的线性组合），但是不知道具体的系数。 因为卡尔曼滤波以实现简单著名，一个重要的原因是它不需要记录历史的所有观测就可以做最优估计，但是到目前为止，我们还不知道怎么迭代性地使用上面的结论。

3. Models For Random Processes: 讲的是对系统进行建模，最理想的情况是首先定义了一个时间原点，然后根据当前时间作为输入给出我们关心的系统输出和时间的数学表达式。这样的方法在实际应用当中不方便，人们更多关注的是基于现在的已知情况，求出当前情况下的系统输出是什么样子（状态转移方程）或者系统变化了多少（一阶微分方程），不管是前面的哪种，都属于一个迭代的描述，从而有了有一阶的微分方程，如果系统是线性的，可以把微分方程写成状态转移方程。但是，尽管我们可以得到状态转移方程，在实际应用当中，无法忽视系统迭代过程中噪声模型，人们也不可能列举所有可能的输入下系统输出的分布情况，但是人们可以统一做一组实验，就是在单位阶跃的输入下，系统随着时间如何变化，或者在均值为0的高斯扰动下，系统产生的输出的不确定度，这个不确定度使用了来表示。需要指出的是，在实际应用中，我们的输入信号已经具有了一定的不确定度，就是，那么经过系统之后，输出的不确定度其实是变大的，也就是论文中公式(24)所要表达的内容。

4. Solution Of the Wiener problem: 讲的是从系统迭代的角度，在有状态转移方程、过程噪声协方差矩阵、测量向量和测量噪声协方差矩阵情况下，如何求解系统状态的最优估计。99%的情况下，新得到的测量值一定是携带了新的有用信息，从数学的角度来看，就是这个一定不在原有的空间内，如果系统运转仍旧保持连续的话，大概率主要部分仍然在内，少部分在正交空间内。方法是把状态向量(经过当前次系统的输出)对的条件期望改写为对和（新增测量向量与形成的正交空间）的条件期望之和（因为两个空间正交，且两个空间合起来组成的manifold和代表的manifold是完全一样的。那么对的条件期望就等于分别对这两个正交空间求条件期望再相加），根据论文之前的所描述的，最后得到一个的矩阵 就得到了卡尔曼增益矩阵。这个矩阵本质上是把新增的这个正交空间内的有用信息加进来形成在全观测下的最优估计。但是到这里，如何计算这个增益矩阵是不知道的，后面根据过程噪声和测量噪声推导卡尔曼增益。

5. 这一步就是需要计算出卡尔曼增益矩阵的具体表达式。因为卡尔曼增益是空间内的向量的线性组合系数，公式(18)表述的最优估计取决于预测和测量两个过程的准确性。直觉上，如果过程噪声小，测量噪声大，那么，最优估计就更接近预测，反之，更接近于测量。 论文中公式(25)卡尔曼增益为什么要那样计算我不明所以，但是我觉得作者心里有一个直观的图形指导他如何推导的。 有必要把前面说的两个噪声画出来，形象理解。经过参考另外一篇论文，卡尔曼滤波的几何解释，作者把状态分为两部分，一部分是观测可以直接测量到的，另一部分是观测无法直接测量的。

我理解的计算过程如下图：图中灰色椭圆代表了新的测量引入的额外信息流形，与蓝色的正交。因为上一时刻的最优估计位于中，预测时候得到的预测量仍然在中。但是根据状态转移方程，实际的状态受到的干扰，并不会严格位于，如图中红色箭头所示。借助于，这个流形正交于（实在是难以画出来），我们手里掌握的信息：预测（黑色向量），预测噪声和预测向量协方差（黑色虚线），测量噪声协方差（绿色虚线），因为当前次的测量决定了最终的结果的投影必然在橙色的直线上，所以问题就转化成了如何根据可测部分和不可测部分，在橙色直线上找到一个点，使得这个点距离红色点最近。很容易知道：过红色点向橙色虚线做垂线，垂足位置就是可观测部分到红色点的最近点，再根据不可观测部分的状态转移方程，不可观测的向量并不完全落入，但是其中的分量是相关的部分，再次对这一部分做垂线得到的点就是不可观测部分的分量。从而我们找到了第次的最优估计。

首先需要明确的是测量误差和经过预测的可观测部分和测量值之间的误差是不同的。

可以用简单的几何关系来推导一下：

推出：

不可观测部分：

推出：

可观测部分

不可观测部分

有了直观的几何理解，后面的具体推导部分，就比较容易理解和建立对应关系了。

具体的推导过程

就是假设随机变量的分布是高斯的，也叫做正态分布，这个分布有如下的特性：

1. 使用表示期望，表示方差，两个参数可以完全描述一个高斯分布
2. 假设过程噪声，测量噪声，两个随机变量期望均为0，协方差矩阵为和

所以过程噪声的概率密度函数写为：

测量噪声的概率密度函数写为：

3. 由于我们需要从第次开始迭代推导，需要首先明确的是状态变量符合什么样的分布？根据论文的描述，在两个前提条件下：

   （1）随机变量和是高斯的，具体说来就是()中的是期望为0，协方差矩阵为的高斯分布，()中的是期望为0，协方差矩阵为的高斯分布。

   （2）最优估计当中的损失函数定义为，第次的最优估计是就是所在空间的基的线性组合，因为这个空间内的基是满足高斯的，所以高斯的线性组合仍然为高斯的：

4. 根据()推导先验概率密度：

高斯分布概率密度函数代入：

两个正太分布的PDF相乘仍然为正态分布的PDF

上面的推导得到结果：

5. 根据()推导后验概率密度：

()推导得到结果：

6. 卡尔曼滤波的五个公式。

()、()、()、()和()就是卡尔曼滤波的向量形式。黑体小写符号代表了向量，大写代表了矩阵。

mathjax 写作遇到的问题

下标经常解析不出来，需要在下标前使用空格，例如f_{k-1} 写成f _{k-1}

参考文献

Data-Driven Science and Engineering -- Steven L. Brunton