卡尔曼公式的基本推导

2023-12-06 11:29:16

卡尔曼公式的基本推导

前一段抽空看了卡尔曼滤波的推导但是无奈数学不好基本忘得差不多了，所以打算记录下来。

卡尔曼滤波（Kalman Filter）

例如我们测量一枚硬币的直径

设第k次测量结果为： \(Z_k\)

第k次	\(Zk\)
1	50.4
2	50.7
3	49.7
4	50.1
5	50.6
...	...
通过多次测量我们可以通过取平均值的方法来估计硬币的真实直径。

设\(x_k\)为第k次的估计值

\(x_k=\frac{1}{k}* \sum_{i=1}^{i=k} \ Z_k\)（1）
\(x_k=\frac{1}{k}* \sum_{i=1}^{i=k-1} \ Z_k+ \frac{1}{k}* \ Z_k\) （2）
\(x_k=\frac{1}{k}*\frac{k-1}{k-1}* \sum_{i=1}^{i=k-1} \ Z_k+ \frac{1}{k}* \ Z_k\)（3）
- \(x_k=\frac{k-1}{k}*\frac{1}{k-1}* \sum_{i=1}^{i=k-1}\ Z_k+ \frac{1}{k}* \ Z_k\)（4）

（4）中 \(\frac{1}{k-1}* \sum_{i=1}^{i=k-1}\ Z_k\)为测量到 \({k-1}\)次时的估计平均值可表示为\(x_{k-1}\)
所以我们可以把（4）化简为

\(x_k=\frac{k-1}{k}*x_{k-1} + \frac{1}{k}* \ Zk\)（5）
\(x_k =x_{k-1} + \frac{1}{k}*( Z_k-x_{k-1}\) )（6）

我们通过（6）梳理一下\(x_k\)与\(x_{k-1}\)、 \(Z_k\)的关系。

当 \(k\)很小时，\(\frac{1}{k}\)比较大， \(Z_k\)的作用就会比较大，对估计平均值仍有较大影响。
当 \(k\)很大时，\(\frac{1}{k}\)趋近于0， \(Z_k\)的作用就会很小，对估计平均值几乎没有影响。（多次测量硬币直径，基本可以确定硬币的真实直径，再次测量对测量的结果不会产生大的影响）

\(\frac{k-1}{k}=K_k\)（7）
\(x_k =x_{k-1} + K_k( Z_k-x_{k-1})\)（8）
当前估计值=上次估计值+\(K_k\)(当前测量值--上次测量值)
\(K_k\):Kalman Qain 卡尔曼增益/卡尔曼因数*

通过最终推导公式可以看出
\(x_k\)与上一次估计平均值\(x_{k-1}\)有关，
而\(x_{k-1}\)又与\(x_{k-2}\)有关
所以卡尔曼滤波的\(x_k\)与\(k\)之前的所有值均有关联，且关系是逐级相关的；
通过公式将数据通过类似链条的方式联系起来。

Kalman Qain：\(K_k=\frac{e_{EST_{k-1}}}{e_{EST_{k-1}}+e_{MEA_k}}\)(9)

\(e_{EST}\):估计误差（\(估计值-真实值\)）
\(e_{MEA}\):测量误差（\(测量值-真实值\)）

在第\(k\)次测量时，
若\(e_{EST_{k-1}}>>e_{MEA}\),根据(9),\(Kk\)接近于1，(6)式\(x_k = Z_k\)
就是第\(k\)次的估计值趋近于第\(k\)次的测量值，(估计误差大的话，我们就选择信任测量值)。
若\(e_{EST_{k-1}}<<e_{MEA}\),根据(9),\(Kk\)接近于0，(6)式\(x_k = x_{k-1}\)
就是第\(k\)次的估计值趋近于第\(k-1\)次的估计值，(测量误差大的话我们选择信任上一次的估计值)。

实际应用三步

计算Kalman Qain：\(K_k=\frac{e_{EST_{k-1}}}{e_{EST_{k-1}}+e_{MEA_k}}\)
计算\(x_k =x_{k-1} + K_k( Z_k-x_{k-1})\)
更新估计误差：\(e_{EST_{k}}=(1-K_k)*e_{EST_{k-1}}\)（后续推导）
假设
硬币实际直径为\(x=50mm\),
第一次的估计值为\(x_0=40mm\)
\(e_{EST}=10mm\)
第一次的测量值为\(z_1=54mm\)
\(e_{MEA}=3mm\)（取决于使用的工具或者传感器）