添加过程噪声
本章中，我们给一维卡尔曼滤波模型加入过程噪声。
一维卡尔曼滤波的完整模型
过程噪声
真实世界中，系统动力模型总是有不确定性的。比如我们想测量一个电阻的阻值，我们假设它是不变的，即阻值不随测量过程而改变。但实际上阻值会随着环境温度的改变而轻微改变。再比如用雷达追踪弹道导弹时，导弹动态模型的不确定性会包含一些随机的加减速。对于飞行器之类的目标，模型不确定性更大，因为飞行员随时可能进行机动。
另一方面，当我们用GPS接收机计算一个固定物体的位置时，由于固定物体不会动所以动态模型不确定性为0. 动态模型的不确定性称为 过程噪声。一些文献中也叫模型噪声、驱动噪声、动态噪声或系统噪声。过程噪声也会带来估计误差。
上一个例子里，我们估计了大楼的高度。由于大楼高度客观上不会改变，我们没有考虑过程噪声。
过程噪声方差 用 \( q \) 表示。
协方差外插方程 应包含 过程噪声方差。
恒定动态模型的协方差外插方程为：
\[ p_{n+1,n}= p_{n,n}+ q_{n} \]
下面为加入了过程噪声后的一维卡尔曼滤波方程：
| 方程 | 方程名 | 文献中可能出现的其他名字 | |
|---|---|---|---|
| 状态更新 | \( K_{n}= \frac{p_{n,n-1}}{p_{n,n-1}+r_{n}} \) | 卡尔曼增益 Kalman Gain | 权重方程 Weight Equation |
| \( \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ K_{n} \left( z_{n}- \hat{x}_{n,n-1} \right) \) | 状态更新 State Update | 滤波方程 Filtering Equation | |
| \( p_{n,n}=~ \left( 1-K_{n} \right) p_{n,n-1} \) | 协方差更新 Covariance Update | 修正方程Corrector Equation | |
| 状态预测 |
\( \hat{x}_{n+1,n}= \hat{x}_{n,n} \) (恒定动态模型) \( \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \) \( \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \) (速度恒定动态模型) |
状态外插 State Extrapolation |
预测器方程 Predictor Equation 转移方程 Transition Equation 预测方程 Prediction Equation 动态模型 Dynamic Model 状态空间模型 State Space Model |
|
\( p_{n+1,n}= p_{n,n} + q_{n} \) (恒定动态模型) \( p_{n+1,n}^{x}= p_{n,n}^{x} + \Delta t^{2} \cdot p_{n,n}^{v} \) \( p_{n+1,n}^{v}= p_{n,n}^{v} + q_{n} \) (速度恒定动态模型) |
协方差外插 Covariance Extrapolation | 预测器协方差方程 Predictor Covariance Equation |
示例 6 - 估计缸中液体的温度
我们想估计一个水缸里的液体的温度。
假设在稳态情况下，液体温度是不变的。但是因为一些微小扰动可能导致液体温度的变化，我们可以用下面的方程描述系统的动态模型：
\[ x_{n}=T+ w_{n} \]
式中：
\( T \) 是恒定不变的温度
\( w_{n} \) 是一个方差为 \( q \) 的随机过程噪声
数值示例
- 假设真实温度是50摄氏度。
- 假设系统模型准确，我们设过程噪声的方差为0.0001.
- 测量误差（标准差）是0.1摄氏度。
- 每5s测量一次。
- 每次测量时真实的液体温度分别为：50.005\( ^{o}C \), 49.994\( ^{o}C \), 49.993\( ^{o}C \), 50.001\( ^{o}C \), 50.006\( ^{o}C \), 49.998\( ^{o}C \), 50.021\( ^{o}C \), 50.005\( ^{o}C \), 50\( ^{o}C \), and 49.997\( ^{o}C \).
- 温度的测量值为：49.986\( ^{o}C \), 49.963\( ^{o}C \), 50.09\( ^{o}C \), 50.001\( ^{o}C \), 50.018\( ^{o}C \), 50.05\( ^{o}C \), 49.938\( ^{o}C \), 49.858\( ^{o}C \), 49.965\( ^{o}C \), and 50.114\( ^{o}C \).
下表比较了真实液体温度和测量值。
第0次迭代
在第一次迭代之前，我们必须先初始化卡尔曼滤波器，并预测下一个状态（即第一个状态）
初始化
我们不知道液体的真实温度是多少，猜测是60\( ^{o}C \).
\[ \hat{x}_{0,0}=60^{o}C \]
我们的猜测是不准确的，所以设它的误差 \( \sigma \) 是100. 则初始化的估计方差为 \( \left( \sigma ^{2} \right) \)：
\[ p_{0,0}=100^{2}=10,000 \]
这个方差非常大（滤波器收敛则很慢）。如果能更可靠地初始化的话，卡尔曼滤波会收敛得相对快一点。
预测
由于我们的模型是恒定的，状态预测等于当前的估计：
\[ \hat{x}_{1,0}=60^{o}C \]
状态预测方差的外插则为：
\[ p_{1,0}= p_{0,0}+q=10000+ 0.0001=10000.0001 \]
第1次迭代
第1步 - 测量
测量值：
\[ z_{1}=~ 49.986^{o}C \]
由于测量误差( \( \sigma \) )是0.1，方差( \( \sigma ^{2} \) )就是0.01：
\[ r_{1}= 0.01 \]
第2步 - 更新
计算卡尔曼增益：
\[ K_{1}= \frac{p_{1,0}}{p_{1,0}+r_{1}}= \frac{10000.0001}{10000.0001+0.01} = 0.999999 \]
卡尔曼增益几乎就是1. 故我们的预测误差远大于测量误差，预测的权重可以忽略不计，而测量的权重几乎就是1.
估计当前状态：
\[ \hat{x}_{1,1}=~ \hat{x}_{1,0}+ K_{1} \left( z_{1}- \hat{x}_{1,0} \right) =60+0.999999 \left( 49.986-60 \right) = 49.986^{o}C \]
更新状态估计方差：
\[ p_{1,1}=~ \left( 1-K_{1} \right) p_{1,0}= \left( 1-0.999999 \right) 10000.0001=0.01 \]
第3步 - 预测
由于系统模型恒定，液体温度不变：
\[ \hat{x}_{2,1}=\hat{x}_{1,1}= 49.986^{o}C \]
状态预测方差的外插为：
\[ p_{2,1}= p_{1,1}+q=0.01+ 0.0001=0.0101 \]
第2次迭代
第1步 - 测量
测量值：
\[ z_{2}=~ 49.963^{o}C \]
由于测量误差( \( \sigma \) )是0.1，方差( \( \sigma ^{2} \) )就是0.01：
\[ r_{2}= 0.01 \]
第2步 - 更新
计算卡尔曼增益：
\[ K_{2}= \frac{p_{2,1}}{p_{2,1}+r_{2}}= \frac{0.0101}{0.0101+0.01} = 0.5 \]
卡尔曼增益是0.5，即预测和测量的权重相等。
估计当前状态：
\[ \hat{x}_{2,2}=~ \hat{x}_{2,1}+ K_{2} \left( z_{2}- \hat{x}_{2,1} \right) =49.986+0.5 \left( 49.963-49.986 \right) =49.974^{o}C \]
更新状态估计方差：
\[ p_{2,2}=~ \left( 1-K_{2} \right) p_{2,1}= \left( 1-0.5 \right) 0.0101=0.005 \]
第3步 - 预测
由于系统模型恒定，液体温度不变：
\[ \hat{x}_{3,2}=\hat{x}_{2,2}= 49.974^{o}C \]
状态预测方差的外插为：
\[ p_{3,2}= p_{2,2}+q=0.005+ 0.0001=0.0051 \]
第3-10次迭代
后续迭代的计算结果汇总在下面的表里：
| \( n \) | \( z_{n} \) | 当前状态估计 ( \( K_{n} \) , \( \hat{x}_{n,n} \) , \( p_{n,n} \) ) | 预测 ( \( \hat{x}_{n+1,n} \) , \( p_{n+1,n} \) ) |
|---|---|---|---|
| 3 | \( 50.09^{o}C \) | \[ K_{3}= \frac{0.0051}{0.0051+0.01}=0.3388 \] \[ \hat{x}_{3,3}=~ 49.974+0.3388 \left( 50.09-49.974 \right) = 50.016^{o}C \] \[ p_{3,3}= \left( 1-0.3388 \right)0.0051 =0.0034 \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3} = 50.016^{o}C \] \[ p_{4,3}= 0.0034+0.0001=0.0035 \] |
| 4 | \( 50.001^{o}C \) | \[ K_{4}= \frac{0.0035}{0.0035+0.01}=0.2586 \] \[ \hat{x}_{4,4}=~ 50.016+0.2586 \left( 50.001-50.016 \right) = 50.012^{o}C \] \[ p_{4,4}= \left( 1-0.2586 \right) 0.0035=0.0026 \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=50.012^{o}C \] \[ p_{5,4}= 0.0026+0.0001=0.0027 \] |
| 5 | \( 50.018^{o}C \) | \[ K_{5}= \frac{0.0027}{0.0027+0.01}=0.2117 \] \[ \hat{x}_{5,5}= 50.012+0.2117 \left( 50.018-50.012 \right) =50.013^{o}C \] \[ p_{5,5}= \left( 1-0.2117 \right) 0.0027=0.0021 \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=50.013^{o}C \] \[ p_{6,5}= 0.0021+0.0001=0.0022 \] |
| 6 | \( 50.05^{o}C \) | \[ K_{6}= \frac{0.0022}{0.0022+0.01}=0.1815 \] \[ \hat{x}_{6,6}=~ 50.013+0.1815 \left( 50.05-50.013 \right) = 50.02^{o}C \] \[ p_{6,6}= \left( 1-0.1815 \right) 0.0022=0.0018 \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=50.02^{o}C \] \[ p_{7,6}= 0.0018+0.0001=0.0019 \] |
| 7 | \( 49.938^{o}C \) | \[ K_{7}= \frac{0.0019}{0.0019+0.01}=0.1607 \] \[ \hat{x}_{7,7}=~ 50.02 + 0.1607 \left( 49.938-50.02 \right) = 50.007^{o}C \] \[ p_{7,7}= \left( 1-0.1607 \right) 0.0019=0.0016 \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=49.978^{o}C \] \[ p_{8,7}= 0.0016+0.0001=0.0017 \] |
| 8 | \( 49.858^{o}C \) | \[ K_{8}= \frac{0.0017}{0.0017+0.01}=0.1458 \] \[ \hat{x}_{8,8}= 50.007+0.1458 \left( 49.858-50.007 \right) =49.985^{o}C \] \[ p_{8,8}= \left( 1-0.1458 \right) 0.0017=0.0015 \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=49.985^{o}C \] \[ p_{9,8}= 0.0015+0.0001=0.0016 \] |
| 9 | \( 49.965^{o}C \) | \[ K_{9}= \frac{0.0016}{0.0016+0.01}=0.1348 \] \[ \hat{x}_{9,9}=~ 49.985+0.1348 \left( 49.965-49.985 \right) =49.982^{o}C \] \[ p_{9,9}= \left( 1-0.1348 \right) 0.0016=0.0014 \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9}=49.982^{o}C \] \[ p_{10,9}= 0.0014+0.0001=0.0015 \] |
| 10 | \( 50.114^{o}C \) | \[ K_{10}= \frac{0.0015}{0.0015+0.01}=0.1265 \] \[ \hat{x}_{10,10}=~ 49.982+0.1265 \left( 50.114 -49.982 \right) =49.999^{o}C \] \[ p_{10,10}= \left( 1-0.1265 \right) 0.0015=0.0013 \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10}=49.999^{o}C \] \[ p_{11,10}= 0.0013+0.0001=0.0014 \] |
结果分析
下图给出了卡尔曼增益的变化。
可见，卡尔曼增益逐渐下降，故卡尔曼滤波收敛了。
下图比较了真值、测量值和估计值。图中置信区间为95%。
置信区间的计算可以在这里找到介绍。
可见，估计值逐渐收敛到真值了，只是其对应的95%置信区间有些过大，说明估计不确定性较高。
示例小结
我们用一维卡尔曼滤波测量了液体的温度。尽管系统动态模型含有随机过程噪声，卡尔曼滤波也给出了较为不错的估计。
示例 7 - 估计加热中的液体的温度 Ⅰ
类似上一个示例，我们估计缸中的液体的温度。本例里，系统的动态模型不是恒定的 - 液体在以0.1\( ^{o}C \)每秒的速度加热。
数值示例
卡尔曼滤波参数和上一个示例类似：
- 假设模型精确，设过程噪声方差 ( \( q \) ) 为 0.0001.
- 测量误差（标准差）为0.1\( ^{o}C \).
- 每5s测量一次。
- 系统动态模型为恒定。
注：尽管真实的动态模型不是恒定的（液体在加热中），我们暂且认为系统动态模型是恒定的（温度不变）。
- 测量点处的真实液体温度分别为：50.505\( ^{o}C \), 50.994\( ^{o}C \), 51.493\( ^{o}C \), 52.001\( ^{o}C \), 52.506\( ^{o}C \), 52.998\( ^{o}C \), 53.521\( ^{o}C \), 54.005\( ^{o}C \), 54.5\( ^{o}C \), and 54.997\( ^{o}C \).
- 测量值为：50.486\( ^{o}C \), 50.963\( ^{o}C \), 51.597\( ^{o}C \), 52.001\( ^{o}C \), 52.518\( ^{o}C \), 53.05\( ^{o}C \), 53.438\( ^{o}C \), 53.858\( ^{o}C \), 54.465\( ^{o}C \), and 55.114\( ^{o}C \).
下图对比了真实液体温度和测量值。
第0次迭代
第0次迭代和上例类似。
第1次迭代前，我们必须初始化卡尔曼滤波，并且预测下一个时刻的状态（即第1个状态）
初始化
我们不知道缸中液体的真是温度，猜测是10\( ^{o}C \).
\[ \hat{x}_{0,0}=10^{o}C \]
这个猜测是不准确的，于是设估计误差 ( \( \sigma \) ) 为 100。初始化的估计方差就是误差方差 \( \left( \sigma ^{2} \right) \)：
\[ p_{0,0}=100^{2}=10,000 \]
这个方差很高，如果选择更可靠的初始值卡尔曼滤波收敛会更快。
预测
现在我们基于初始值来预测下一个状态。由于模型是恒定的，预测值就等于当前的估计值：
\[ \hat{x}_{1,0}=10^{o}C \]
状态预测方差的外插为：
\[ p_{1,0}= p_{0,0}+q=10000+ 0.0001=10000.0001 \]
第1-10次迭代
后续迭代的情况汇总在下面的表里：
| \( n \) | \( z_{n} \) | 当前状态估计 ( \( K_{n} \) , \( \hat{x}_{n,n} \) , \( p_{n,n} \) ) | 预测 ( \( \hat{x}_{n+1,n} \) , \( p_{n+1,n} \) ) |
|---|---|---|---|
| 1 | \( 50.486^{o}C \) | \[ K_{1}= \frac{10000.0001}{10000.0001+0.01} = 0.999999 \] \[ \hat{x}_{1,1}=~ 10+0.999999 \left( 50.486-10 \right) = 50.486^{o}C \] \[ p_{1,1}= \left( 1-0.999999 \right) 10000.0001=0.01 \] | \[ \hat{x}_{2,1}= \hat{x}_{1,1}=50.486^{o}C \] \[ p_{2,1}= 0.01+0.0001=0.0101 \] |
| 2 | \( 50.963^{o}C \) | \[ K_{2}= \frac{0.0101}{0.0101+0.01}=0.5025 \] \[ \hat{x}_{2,2}=~ 50.486+0.5025 \left( 50.963-50.486 \right) =50.726^{o}C\] \[ p_{2,2}= \left( 1-0.5025 \right) 0.0101=0.005 \] | \[ \hat{x}_{3,2}= \hat{x}_{2,2}=50.726^{o}C \] \[ p_{3,2}= 0.005+0.0001=0.0051 \] |
| 3 | \( 51.597^{o}C \) | \[ K_{3}= \frac{0.0051}{0.0051+0.01}=0.3388 \] \[ \hat{x}_{3,3}=~ 50.726+0.3388 \left( 51.597-50.726 \right) = 51.021^{o}C\] \[ p_{3,3}= \left( 1-0.3388 \right) 0.0051=0.0034 \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3}=51.021^{o}C \] \[ p_{4,3}= 0.0034+0.0001=0.0035 \] |
| 4 | \( 52.001^{o}C \) | \[ K_{4}= \frac{0.0035}{0.0035+0.01}=0.2586 \] \[ \hat{x}_{4,4}=~ 51.021+0.2586 \left( 52.001-51.021 \right) =51.274^{o}C \] \[ p_{4,4}= \left( 1-0.2586 \right) 0.0035=0.0026 \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=51.274^{o}C \] \[ p_{5,4}= 0.0026+0.0001=0.0027 \] |
| 5 | \( 52.518^{o}C \) | \[ K_{5}= \frac{0.0027}{0.0027+0.01}=0.2117 \] \[ \hat{x}_{5,5}= 51.274+0.2117 \left( 52.518-51.274 \right) =51.538^{o}C \] \[ p_{5,5}= \left( 1-0.2117 \right) 0.0027=0.0021 \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=51.538^{o}C \] \[ p_{6,5}= 0.0021+0.0001=0.0022 \] |
| 6 | \( 53.05^{o}C \) | \[ K_{6}= \frac{0.0022}{0.0022+0.01}=0.1815 \] \[ \hat{x}_{6,6}=~ 51.538+0.1815 \left( 53.05-51.538 \right) = 51.812^{o}C \] \[ p_{6,6}= \left( 1-0.1815 \right) 0.0022=0.0018 \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=51.812^{o}C \] \[ p_{7,6}= 0.0018+0.0001=0.0019 \] |
| 7 | \( 53.438^{o}C \) | \[ K_{7}= \frac{0.0019}{0.0019+0.01}=0.1607 \] \[ \hat{x}_{7,7}=~ 51.812+0.1607 \left( 53.438-51.812 \right) =52.0735^{o}C \] \[ p_{7,7}= \left( 1-0.1607 \right) 0.0019=0.0016 \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=52.0735^{o}C \] \[ p_{8,7}= 0.0016+0.0001=0.0017 \] |
| 8 | \( 53.858^{o}C \) | \[ K_{8}= \frac{0.0017}{0.0017+0.01}=0.1458 \] \[ \hat{x}_{8,8}= 52.0735+0.1458 \left( 53.858-52.0735 \right) =52.334^{o}C \] \[ p_{8,8}= \left( 1-0.1458 \right) 0.0017=0.0015 \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=52.334^{o}C \] \[ p_{9,8}= 0.0015+0.0001=0.0016 \] |
| 9 | \( 54.523^{o}C \) | \[ K_{9}= \frac{0.0016}{0.0016+0.01}=0.1348 \] \[ \hat{x}_{9,9}=~ 52.334+0.1348 \left( 54.523-52.334 \right) =52.621^{o}C \] \[ p_{9,9}= \left( 1-0.1348 \right) 0.0016=0.0014 \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9}=52.621^{o}C \] \[ p_{10,9}= 0.0014+0.0001=0.0015 \] |
| 10 | \( 55.114^{o}C \) | \[ K_{10}= \frac{0.0015}{0.0015+0.01}=0.1265 \] \[ \hat{x}_{10,10}=~ 2.621+0.1265 \left( 55.114 -52.621 \right) =52.936^{o}C \] \[ p_{10,10}= \left( 1-0.1265 \right) 0.0015=0.0013 \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10}=52.936^{o}C \] \[ p_{11,10}= 0.0013+0.0001=0.0014 \] |
结果分析
下图中比较测量值、估计值以及真值。
可见，卡尔曼滤波没能给出一个合理的估计，其估计产生了滞后误差。在示例3里，用 \( \alpha - \beta \) 滤波器估计加速中的飞行器时我们已经遇到过一次滞后误差。在示例4里我们通过将 \( \alpha - \beta \) 滤波器更换为能处理加速度估计的 \( \alpha -\beta -\gamma \) 滤波器消除了滞后误差。
刚才的卡尔曼滤波中之所以产生滞后误差，有两个原因：
- 动态模型和实际不符。
- 我们设定了过低的过程噪声 \( \left( q=0.0001 \right) \)，而真实的温度变化要远大于这个值。
有两个方法能消除滞后误差：
- 其一，如果我们能预先知道液体温度是线性变化的，就可以设计一个考虑温度线性增长的动态模型。在示例 4中我们就是这样做的，也是一般情况下推荐采用的方法。但是如果温度变化的规律没办法建模，那这种手段就不能改善卡尔曼滤波的性能。
- 其二，既然是模型失准了，我们可以把模型差异当作一种噪声，可以提高模型的过程噪声 \( \left( q \right) \)，下一个示例会具体讲这种方法。
另一个问题是估计值明明不对，但不确定性却很低（置信区间很窄）。这说明卡尔曼滤波盲目信任预测值，无法给出更准确的估计值。这些现象说明这是一个没设计好的卡尔曼滤波器。
示例小结
本例中，我们用一个面向恒定模型的一维卡尔曼滤波器估计了一个正在加热中的液体的温度。我们观察到卡尔曼滤波的输出产生了滞后误差，这个滞后误差是由错误的动态模型所带来的。
合理的模型设计能够消除滞后误差。
示例8 - 估计加热中的液体的温度 Ⅱ
与前一个示例类似，只有一点改变：由于动态模型不是很匹配，我们把过程噪声方差 \( \left( q \right) \) 从 0.0001 增大到 0.15.
数值示例
第0次迭代
第1次迭代前，我们必须初始化卡尔曼滤波，并且预测下一个时刻的状态（即第1个状态）
初始化
初始化过程和上例类似。
我们不知道缸中液体的真是温度，猜测是10\( ^{o}C \).
\[ \hat{x}_{0,0}=10^{o}C \]
这个猜测是不准确的，于是设估计误差 ( \( \sigma \) ) 为 100。初始化的估计方差就是误差方差 \( \left( \sigma ^{2} \right) \)：
\[ p_{0,0}=100^{2}=10,000 \]
这个方差很高，如果选择更可靠的初始值卡尔曼滤波收敛会更快。
Prediction
现在我们基于初始值来预测下一个状态。由于模型是恒定的，预测值就等于当前的估计值：
\[ \hat{x}_{1,0}=10^{o}C \]
状态预测方差的外插为：
\[ p_{1,0}= p_{0,0}+q=10000+ 0.15=10000.15 \]
第1-10次迭代
后续迭代的情况汇总在下面的表里：
| \( n \) | \( z_{n} \) | 当前状态估计 ( \( K_{n} \) , \( \hat{x}_{n,n} \) , \( p_{n,n} \) ) | 预测 ( \( \hat{x}_{n+1,n} \) , \( p_{n+1,n} \) ) |
|---|---|---|---|
| 1 | \( 50.486^{o}C \) | \[ K_{1}= \frac{10000.15}{10000.15+0.01} = 0.999999 \] \[ \hat{x}_{1,1}=~ 10+0.999999 \left( 50.486-10 \right) =50.486^{o}C \] \[ p_{1,1}= \left( 1-0.999999 \right)10000.15=0.01 \] | \[ \hat{x}_{2,1}= \hat{x}_{1,1}=50.486^{o}C \] \[ p_{2,1}= 0.01+0.15=0.16 \] |
| 2 | \( 50.963^{o}C \) | \[ K_{2}= \frac{0.16}{0.16+0.01}=0.9412 \] \[ \hat{x}_{2,2}=~ 50.486+0.9412 \left( 50.963-50.486 \right) =50.934^{o}C \] \[ p_{2,2}= \left( 1-0.9412 \right) 0.16=0.0094 \] | \[ \hat{x}_{3,2}= \hat{x}_{2,2}=50.934^{o}C \] \[ p_{3,2}= 0.0094+0.15=0.1594 \] |
| 3 | \( 51.597^{o}C \) | \[ K_{3}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{3,3}=~ 50.934+0.941 \left( 51.597-50.934 \right) =51.556^{o}C\] \[ p_{3,3}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3}=51.556^{o}C \] \[ p_{4,3}= 0.0094+0.15=0.1594 \] |
| 4 | \( 52.001^{o}C \) | \[ K_{4}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{4,4}=~ 51.556+0.941 \left( 52.001-51.556 \right) = 51.975^{o}C \] \[ p_{4,4}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=51.975^{o}C \] \[ p_{5,4}= 0.0094+0.15=0.1594 \] |
| 5 | \( 52.518^{o}C \) | \[ K_{5}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{5,5}= 51.975+0.941 \left( 52.518-51.975 \right) =52.486^{o}C \] \[ p_{5,5}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=52.486^{o}C \] \[ p_{6,5}= 0.0094+0.15=0.1594 \] |
| 6 | \( 53.05^{o}C \) | \[ K_{6}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{6,6}=~ 52.486+0.941 \left( 53.05-52.486 \right) =53.017^{o}C \] \[ p_{6,6}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=53.017^{o}C \] \[ p_{7,6}= 0.0094+0.15=0.1594 \] |
| 7 | \( 53.438^{o}C \) | \[ K_{7}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{7,7}=~ 53.017+0.941 \left( 53.438-53.017 \right) =53.413^{o}C \] \[ p_{7,7}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=53.413^{o}C \] \[ p_{8,7}= 0.0094+0.15=0.1594 \] |
| 8 | \( 53.858^{o}C \) | \[ K_{8}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{8,8}= 53.413+0.941 \left( 53.858-53.413 \right) =53.832^{o}C \] \[ p_{8,8}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=53.832^{o}C \] \[ p_{9,8}= 0.0094+0.15=0.1594 \] |
| 9 | \( 54.523^{o}C \) | \[ K_{9}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{9,9}=~ 53.832+0.941 \left( 54.523-53.832 \right) =54.428^{o}C \] \[ p_{9,9}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9}=54.428^{o}C \] \[ p_{10,9}= 0.0094+0.15=0.1594 \] |
| 10 | \( 55.114^{o}C \) | \[ K_{10}= \frac{0.1594}{0.1594+0.01}=0.941 \] \[ \hat{x}_{10,10}=~ 54.428+0.941 \left( 55.114 -54.428 \right) =55.074^{o}C \] \[ p_{10,10}= \left( 1-0.941 \right) 0.1594=0.0094 \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10}=55.074^{o}C \] \[ p_{11,10}= 0.0094+0.15=0.1594 \] |
结果分析
下图中比较测量值、估计值以及真值。
可以看到，估计值跟上了测量值，滞后误差被消除了。
把过程不确定性加大能够消除稳态误差，但是由于模型仍然是不准的，过程不确定性变大会让卡尔曼滤波更相信测量，因此最终的估计值会几乎等于充满噪声的测量值，整个滤波的目的就达不到了。
我们看看卡尔曼增益的情况。
由于过程不确定性很大，测量值的权重远比估计值的权重高，即卡尔曼增益很高，最终收敛到了0.94.
好消息是本例中测量值都还不错，真值（绿线）基本都落在估计值的95%置信区间内了。
示例小结
一个完美的卡尔曼滤波器要求系统的动态模型非常精确，尽量降低过程噪声。但有时候精确的模型就是无法获得的，比如飞机的飞行员随时都可能突然进行机动，从而大幅改变飞机的轨迹，这是无法预料和建模的，过程噪声就是很大。