算法-卡尔曼滤波之引入噪声

前面我们的例子引出了测量不确定和状态不确定性，但是在实际过程中，系统动态模型也存在不确定性；

这里的过程噪声是一个不确定性，而此前的协方差外推方程就是根据不确定推导的来的；

 

外推方程和协方差外推方程取决于系统是静态还是动态的。 

例如测量罐子中液体的温度：我们假设真实的温度是50摄氏度，并且模型是很标准的，此时我们将过程噪声的方差设置为0.0001，测量误差是0.1摄氏度；

 使用更新，预测来进行

首先，在第零次迭代，在进行第一次迭代之前，我们需要初始化卡尔曼滤波器，然后根据外推方程和协方差外推方程去预测下一个时刻的状态和估计不确定性；

初始化：此时不知道罐内液体温度，我们假设此时液体温度是10℃，第0时刻的估计值是10℃；

预测：由于我们的初始值是猜测值是非常不准确，此时初始估计误差标准差设置为100，估计不确定性的方差就是10000；

初始估计不确定性=100*100=10000；

基于初始值来预测下一时刻的状态，由于我们认定此时系统模型是静态的，预测估计与当前估计相同；

外推不确定性（方差）=初始估计不确定性+过程噪声=10000+0.0001=10000.0001；

此时我们有了协方差外推方程的结果（估计不确定性+误差），外推方程的结果（静态模型，前后两次状态保持一致）；

 第一次迭代：

测量：经过测量得到 z1=49.95摄氏度，由于测量误差为0.1，方差为0.01，测量不确定性：r1=0.01；

更新：计算卡尔曼增益，k=估计不确定性+误差/估计不确定性+误差+测量不确定性；

计算的K=0.999999；

此时卡尔曼增益几乎为1，说明估计误差远远大于测量误差；

当前状态估计：

当前状态（第一次）=上一时刻的状态（第0次初始化的结果）+K（卡尔曼增益）（测量值-上一时刻的状态）；

更新当前估计不确定性：下一次的估计不确定性=（1-K）当前估计不确定性；

预测：由于是静态模型，液体的温度不变；

第二次预测结果等于当前估计的结果；

外推估计不确定性；

 

 第二次迭代：

测量：经过测量得到 z1=49.967摄氏度，由于测量误差为0.1，方差为0.01，测量不确定性：r1=0.01；

更新：计算卡尔曼增益，k=估计不确定性+误差/估计不确定性+误差+测量不确定性；

计算的K=0.5；

更新:

当前状态估计：

当前状态（第一次）=上一时刻的状态（第0次初始化的结果）+K（卡尔曼增益）（测量值-上一时刻的状态）；

更新当前估计不确定性：下一次的估计不确定性=（1-K）当前估计不确定性；

预测：由于是静态模型，液体的温度不变；

第二次预测结果等于当前估计的结果；

外推估计不确定性；

 重复：测量->更新->预测-测量.....得到如下结果：

 

 

卡尔曼增益：

 

静态模型中我们引入了过程噪声，过程噪声是系统模型的误差，也就是一种不确定性，因为，我们修改了由不确定性组件的方程，协方差外推方程，我们加上了这个不确定性；

现在罐子内液体的温度在以每秒0.1℃进行加热，但是我们此时认为该模型仍然是静态的，即温度不会发送变化，此时认为假设是正确的，过程噪声方差q很小为0.0001，测量误差是0.1（标准差），使用这样的模型去预测动态温度，发现效果不是很准确；

所以我们发现，当我们试验误差较大时候，可以引入增大过程噪声，把不确定性包含进来；

在模型存在误差的时候，但是我们认为系统不存储误差，并设置过程噪声为0.0001，仍然使用上面的方法进行估计：

 

我们发现预测结果和真实值，随着迭代次数增加，存在滞后误差；

 

通过我们上面的分析，我们得到，产生滞后误差的原因：

 我们采用方法2，修改过程噪声，设置为0.15：