对静态系统进行估计

静态系统就是，在相当时间内其本身属性（状态）不会随着时间的变化而变化。

我们再次把那颗125克的钻石拿出来，钻石是世界上最坚硬的自然物质，假设其重量125克从现在起永远不会发生变化了。 我们再次给他称重，这次我们不用老式台称了，换一个最先进的称，它理论上没有系统误差（在没有其他干扰的情况下是准确的） 。但是不出意外的话还是出意外了，我们在称的时候受到了微小的干扰（即随机噪声），所以我们的测量值是不准确的。

为了能逼近到他的准确值，我们对它进行多次测量，取平均值

\[\hat{x}_{n,n}= \frac{1}{n} \left( z_{1}+ z_{2}+ \ldots + z_{n-1}+ z_{n} \right) = \frac{1}{n} \sum _{i=1}^{n} \left( z_{i} \right)\]

• \(x\)--------- 钻石的准确重量

• \(z_{n}\)--------- n时刻钻石的测量值

• \(\hat{x}_{n,n}\) --------- 使用n时刻的测量值 \(z_{n}\)，对n时刻 \(x\) 的估计值

• \(\hat{x}_{n+1,n}\) --------- n时刻对未来时刻(n+1时刻) \(x\) 的预测值

• \(\hat{x}_{n-1,n-1}\)--------- 使用n-1时刻的测量值 \(z_{n-1}\) 对n-1时刻， \(x\) 的估计值

• \(\hat{x}_{n,n-1}\)--------- 在n-1时刻对n时刻状态的预测，对n时刻来说是它的先验估计

Note

变量上的尖号符号代表这是一个对该变量的估计值,。\(\hat{x}\) 代表对 \(x\) 的估计。

讲道理来说，钻石的重量是不变的，它n时刻的重量是等于n+1时刻的重量的。数学建模是：

\[\hat{x}_{n+1,n}= \hat{x}_{n,n}\]

对于计算机而言，利用平均值求解逼近真实值，是没问题的。但是每次我们新称重一下传给计算机后，计算机都要将最新的 数据储存下来，重新将所有数据加在一起求解平均值，显然对计算机算力的消耗是巨大且浪费的

我们只需要储存上一个时刻的估计值就好，这一个时刻测量完后将其覆盖更新即可

• 根据当前的测量和先验估计，估计当前的状态。

• 根据当前的状态估计以及系统动态模型，进行下一时刻的预测。

将求平均的表达式进行等效变换

\(\hat{x}_{n,n}= \frac{1}{n} \sum _{i=1}^{n} \left( z_{i} \right)\)---------求平均公式

\(= \frac{1}{n} \left( \sum _{i=1}^{n-1} \left( z_{i} \right) + z_{n} \right)\)---------前n-1个测量的和再加上最近一次的测量值整体除以n

\(= \frac{1}{n} \sum _{i=1}^{n-1} \left( z_{i} \right) + \frac{1}{n} z_{n}\)---------\(\frac{1}{n}\) 乘进去展开

\(= \frac{1}{n}\frac{n-1}{n-1} \sum _{i=1}^{n-1} \left( z_{i} \right) + \frac{1}{n} z_{n}\)---------给求和项同时乘以并除以n-1

\(= \frac{n-1}{n}\color{#00008b}{\frac{1}{n-1} \sum _{i=1}^{n-1} \left( z_{i} \right)} + \frac{1}{n} z_{n}\)---------着色项就是上一时刻的估计值

\(= \frac{n-1}{n}\color{#00008b}{\hat{x}_{n-1,n-1}} + \frac{1}{n} z_{n}\)----------把求和项用上一时刻的估计值替换掉

\(= \hat{x}_{n-1,n-1}- \frac{1}{n}\hat{x}_{n-1,n-1}+ \frac{1}{n} z_{n}\)---------\(\hat{x}_{n-1,n-1}\) 乘进 \(\frac{n-1}{n}\) 的分子

\(= \hat{x}_{n-1,n-1}+ \frac{1}{n} \left( z_{n}- \hat{x}_{n-1,n-1} \right)\)---------提出 \(\frac{1}{n}\)

之前我们说了，讲道理来说，钻石的重量是不变的，它n时刻的重量是等于n+1时刻的重量的。

\[\hat{x}_{n,n-1}= \hat{x}_{n-1,n-1}\]

对当前时刻状态 \(\hat{x}_{n,n}\) 的估计可以写成

\[\hat{x}_{n,n} = \hat{x}_{n,n-1} + \frac{1}{n} \left( z_{n} - \hat{x}_{n,n-1} \right)\]

Note

上面的式子即为卡尔曼滤波的五个方程之一。称为状态更新方程。 将系数 \(\frac{1}{n}\) 用 \(K_{n}\) 替换。 在卡尔曼滤波中这个系数是一个重要的概念 卡尔曼增益 它随着n的迭代而迭代。

\(\left( z_{n}- \hat{x}_{n,n-1} \right)\) 叫更新量

实例推导

下面我们会以钻石为例实际验证一下上面的估计公式，或者说算法。

卡尔曼滤波需要一个初始估计作为初始值，这个值可以非常粗略，甚至是0。

初始化（第0次迭代）

买钻石的时候我们被告知它125g，我们就拿着个作为初始值。仅在滤波器初始化时使用一次，后续迭代不再需要这个值。

\[\hat{x}_{0,0}=125g\]

预测

模型是静态的，对下一个时刻的预测就等于此时的初始化估计值。

\[\hat{x}_{1,0} = \hat{x}_{0,0}=125g\]

第1次迭代

• 第一步,称重：

\[z_{1}= 124.7g\]

• 第二步，计算增益，\(K_{n} = \frac{1}{n}\) 。所以：

\[K_{n} = \frac{1}{1} = 1\]

用状态更新方程计算当前的估计值:

\[\hat{x}_{1,1}= \hat{x}_{1,0}+ K_{1} \left( z_{1}- \hat{x}_{1,0} \right) = 125+1 \left( 124.7-125 \right) = 124.7g\]

• 第三步

模型是静态的，钻石的重量应该是不变的，对下一时刻的预测应该等于此时的估计：

\[\hat{x}_{2,1}= \hat{x}_{1,1}=124.7g\]

第2次迭代

上一时刻的预测值成为了这一时刻的先验估计

\[\hat{x}_{2,1}=124.7g\]

• 第1步,第二次称重：

\[z_{2}= 125.3g\]

• 第2步,计算增益：

\[z_{2}= 125.3g\]

计算估计值：

\[\hat{x}_{2,2}= \hat{x}_{2,1}+ K_{2} \left( z_{2}- \hat{x}_{2,1} \right) =124.7+\frac{1}{2} \left( 125.3-124.7 \right) = 125g\]

• 第3步

\[\hat{x}_{3,2}= \hat{x}_{2,2}=125g\]

第三次迭代

\[ \begin{align}\begin{aligned}z_{3}= 124.9g~~~~~~~~~~~~~~~~~~~~~~~~~~ K_{3}= \frac{1}{3}\\\hat{x}_{3,3}=~ 125+\frac{1}{3} \left( 124.9-125 \right) =124.97g\end{aligned}\end{align} \]

\[\hat{x}_{4,3}=124.97g\]

第4次迭代

\[ \begin{align}\begin{aligned}z_{4}= 125.1g~~~~~~~~~~~~~~~~~~~~~~~~~~ K_{4}= \frac{1}{4}\\\hat{x}_{4,4}=~ 124.97+\frac{1}{4} \left( 125.1-124.97 \right) =125.0025g\end{aligned}\end{align} \]

\[\hat{x}_{5,4}=125.0025g\]

第5次迭代

\[ \begin{align}\begin{aligned}z_{5}= 125g~~~~~~~~~~~~~~~~~~~~~~~~~~ K_{5}= \frac{1}{5}\\\hat{x}_{5,5}=~ 125+\frac{1}{5} \left( 125-125.0025\right) =125.0005g\end{aligned}\end{align} \]

\[\hat{x}_{6,5}=125.0005\]

明显可以看到随着迭代的次数增加，我们得到的预测值越来越接近真实值了。

作业

将上述步骤以编程的方式实现，并将真实值，估计值，测量值，绘制成图表。