算法介绍及实现——卡尔曼滤波( 三 ) _噪声

——step3完成当前状态的估计之后，基于当前状态估计值使用预测方程获得对下一状态的预测值。
——step4重复步骤2和步骤3 。
在建模过程中，g-h-k滤波器的参数选择至关重要。
g_h_k滤波器优化论文：
（文末获取）
三、一维卡尔曼滤波器1）引入观测噪声（ Error）
回顾引例2，在对黄金的称重过程中，其结果如下：
我们可以明显的发现测量值和真实值之间存在很大的差距，我们将这种差距称为测量误差，也就是测量噪声。我们将测量误差用测量不确定度来表示，记为r 。在本例中，我们可以通过称的商家获得（商家一般都会有自己仪器的测量误差）或推导获得。假设方差为m*m，那么标准差为m，根据随机误差服从正态分布可得下图：
由正态分布理论可知，若真值属于观测值左右一个标准差范围内（l-m，l+m），则认为观测是合理的，上图10次观测中，有8次符合条件。所以，测量的不确定都r即为方差m*m 。
现在推导卡尔曼增益方程，在原来的引例中，参数取的是1/n，在卡尔曼滤波过程中，卡尔曼增益是迭代计算的，如下式即为卡尔曼增益方程：
其中，r为测量不确定度，p为外推估计的不确定度（用于衡量估计值与真实值的误差，可理解为外推估计的权重）。r为方差大于零，所以卡尔曼增益范围为0至1 。
综上状态更新方程可变为：
外推估计的不确定（p）的更新
上式被称为协方差更新方程，每次滤波器迭代时，1-K是小于1的，所以估计的不确定性在减小。当测量不确定度（r，等于方差）很大时，K值很小，1-K就更接近1，所以滤波器收敛速度慢，反之。
现在，我们来讨论估计不确度的外推。首先我们梳理一下，当上一状态完成外推估计（预测）后，获得外推估计不确定度Pn,n-1，进而计算出卡尔曼增益K，再根据卡尔曼增益获得当前估计的不确定度Pn,n 。而我们当前状态仍然需要对下一状态进行外推估计（预测），所以我们需要计算出Pn+1,n 。
以引例3匀速飞行飞机为例，状态包括位移和速度，其状态外推方程为:
基于外推方程可以得到估计的不确定度计算公式：
（关于协方差外推公式的理解：如下图）

文章插图
估计不确定度的外推方程称为协方差外推方程。至此五个卡尔曼方程介绍结束。
在此，我们将所学的五个卡尔曼方程汇总起来：
首先是状态更新方程：实现对预测值和观测值的加权拟合（滤波）。
第二个是状态外推（预测）方程：基于当前状态估计值对下一个状态进行估计。
（以引例3为例）
第三是在引入观测噪声后，计算卡尔曼增益的卡尔曼增益方程。
第四是对估计不确定度的更新方程，即协方差更新方程。
第五是将估计不确定度外推到下一状态的协方差外推方程，（以引例3为例）
例子可见官网。
2）引入过程噪声（ Noise）
综上所述，我们讨论了含有观测噪声的情况下的卡尔曼滤波建模方法，并做了算例分析。但正如最先所讲，不仅包括观测噪声，还包括过程噪声，动态模型（状态外推方程）的不确定性称为过程噪声，只有都考虑到才是完整的卡尔曼滤波模型。
从理论上讲，过程噪声造成的影响是使得估计的不确定度（p）变大，所以在协方差外推方程中，需要加入过程噪声的影响，因为观测噪声和过程噪声显然不相关，所以直接加上。
如下所列为恒定状态下：
对于引例3而言，对于位移外推的不确定度如下：