卡尔曼滤波器：1.递归算法(recursive algorithm)
简介
滤波器并不能代表卡尔曼的内核精神，卡尔曼滤波器的最优解释是optimal recursive data processing algorithm（最优化递归数字处理算法），更像是一种观测器，有着很广泛的应用，这个大量应用主要是因为我们的世界存在着很大的不确定性，这种不确定性体现在是以下三个方面：
1.不存在完美的数学模型
2.系统的扰动不可控，也很难进行建模
3.测量传感器存在着误差
在例子中推导
用一把尺子测量一个硬币，用 z k z_k zk表示测量结果，下标 k k k就代表了第 k k k次的测量结果
每个人的测量不同，尺子也存在着误差
z
1
=
50.1
m
m
z
2
=
50.4
m
m
z
3
=
50.2
m
m
\begin{align} &z_1 = 50.1mm\\ &z_2 = 50.4mm\\ &z_3 = 50.2mm \end{align}
z1=50.1mmz2=50.4mmz3=50.2mm
这时候让估计这个硬币的真实数据，很自然的就会想到取平均值
x
^
k
=
1
k
(
z
1
+
z
2
+
z
3
+
.
.
.
+
z
k
)
=
1
k
(
z
1
+
z
2
+
z
3
+
.
.
.
.
+
z
k
−
1
)
+
1
k
z
k
=
k
−
1
k
1
k
−
1
(
z
1
+
z
2
+
z
3
+
.
.
.
.
+
z
k
−
1
)
+
1
k
z
k
=
k
−
1
k
x
^
k
−
1
+
1
k
z
k
=
x
^
k
−
1
+
1
k
(
z
k
−
x
^
k
−
1
)
\begin{align*} &\widehat{x}_k = \frac{1}{k}(z_1 + z_2 + z_3 + ... +z_k)\\ &=\frac{1}{k}(z_1 + z_2 + z_3 + .... + z_{k-1}) + \frac{1}{k}z_k\\ &=\frac{k-1}{k}\frac{1}{k-1}(z_1 + z_2 + z_3 + .... + z_{k-1}) + \frac{1}{k}z_k\\ &=\frac{k-1}{k}\widehat{x}_{k-1} + \frac{1}{k}z_k\\ &= \widehat{x}_{k-1} + \frac{1}{k}(z_k - \widehat{x}_{k-1}) \end{align*}
x
k=k1(z1+z2+z3+...+zk)=k1(z1+z2+z3+....+zk−1)+k1zk=kk−1k−11(z1+z2+z3+....+zk−1)+k1zk=kk−1x
k−1+k1zk=x
k−1+k1(zk−x
k−1)
随着k的增加，
1
k
\frac{1}{k}
k1趋近0，这时候，
x
^
k
−
>
x
^
k
−
1
\widehat{x}_k->\widehat{x}_{k-1}
x
k−>x
k−1，随着大量的数据增加，意味着测量不在重要
当k值较小的时候 1 k \frac{1}{k} k1较大，测量值就体现出其价值了
将上述公式写成下面的样子
x
^
k
=
x
^
k
−
1
+
K
k
(
z
k
−
x
^
k
−
1
)
\widehat{x}_k = \widehat{x}_{k-1} + K_k(z_k - \widehat{x}_{k-1})
x
k=x
k−1+Kk(zk−x
k−1)
代表的含义就是：
当前的估计值 = 上一时刻的估计值 + 系数 * （当前测量值 - 上一此的估计值）
K k K_k Kk:kalman gain 卡尔曼增益（因数）
这就是递归的思想，这也是卡尔曼的优势，不需要追溯到很久之前，只需要知道上一时刻的就行
对卡尔曼增益 K k K_k Kk的简单介绍
首先引入两个概念：
估计误差： e E S T e_{EST} eEST esitimate
测量误差：
e
M
E
A
e_{MEA}
eMEA measurement
K
k
=
e
E
S
T
k
−
1
e
E
S
T
k
−
1
+
e
M
E
A
k
K_k = \frac{e_{EST_{k-1}}}{e_{EST_{k-1}} + e_{MEA_k}}
Kk=eESTk−1+eMEAkeESTk−1
后面会增加推导过程
先讨论一下，
当 k k k时刻，
1. e E S T k − 1 > > e M E A k : K k − > 1 1.e_{EST_{k-1}} >> e_{MEA_k}:K_k->1 1.eESTk−1>>eMEAk:Kk−>1 x ^ k = z k \widehat{x}_k = z_k x k=zk 估计的误差大，更加相信测量值
2. e M E A k > > e E S T k − 1 : K k − > 0 2.e_{MEA_k} >> e_{EST_{k-1}} : K_k->0 2.eMEAk>>eESTk−1:Kk−>0 x ^ k = x ^ k − 1 \widehat{x}_k = \widehat{x}_{k-1} x k=x k−1 测量误差大，更加相信估计值
解决问题步骤
step1.计算卡尔曼增益 K k = e E S T k − 1 e E S T k − 1 + e E M A k K_k = \frac{e_{EST_{k-1}}}{e_{EST_{k-1}}+e_{EMA_k}} Kk=eESTk−1+eEMAkeESTk−1
step2.计算估计值 x ^ k = x ^ k − 1 + K k ( z k − x ^ k − 1 ) \widehat{x}_k = \widehat{x}_{k-1} + K_k(z_k - \widehat{x}_{k-1}) x k=x k−1+Kk(zk−x k−1)
step3.更新估计误差 e E S T k = ( 1 − K k ) e E S T k − 1 e_{EST_k} = (1 - K_k)e_{EST_{k-1}} eESTk=(1−Kk)eESTk−1 后面增加推导过程
假设硬币的实际长度是 50 m m 50mm 50mm,
估计值： x ^ 0 = 40 m m \widehat{x}_0 = 40mm x 0=40mm 第一次可随便设置（因为不知道实际的长度）
估计误差： e E S T 0 = 5 m m e_{EST_0} = 5mm eEST0=5mm 第一次也是随便给的初始值
测量值： z 1 = 51 m m z_1 = 51mm z1=51mm
测量误差： e M E A k = 3 m m e_{MEA_k} = 3mm eMEAk=3mm
| k k k | z k z_k zk | e M E A k e_{MEA_k} eMEAk | x ^ k \widehat{x}_k x k | K k K_k Kk | e E S T k e_{EST_k} eESTk |
|---|---|---|---|---|---|
| 0 | 40 | 5 | |||
| 1 | 51 | 3 | 46.875 | 0.625 | 1.878 |
| 2 | 48 | 3 | 47.875 | 0.3846 | 1.154 |
| 3 | … | … | … | … |
k = 1 : K k = 5 5 + 3 = 0.625 x ^ k = 40 + 0.625 ( 51 − 40 ) = 46.875 e E S T k = ( 1 − 0.625 ) 5 = 1.878 k = 2 : K k = 1.878 1.878 + 3 = 0.3846 x ^ k = 46.875 + 0.3846 ( 48 − 46.875 ) = 47.308 e E S T k = ( 1 − 0.3846 ) 1.878 = 1.54 . . . \begin{align*}{} k=1:&K_k = \frac{5}{5+3} = 0.625 \\ &\widehat{x}_k = 40 + 0.625(51 - 40) = 46.875 \\ &e_EST_k = (1 - 0.625)5=1.878\\ k=2:&K_k = \frac{1.878}{1.878+3} = 0.3846 \\ &\widehat{x}_k = 46.875 + 0.3846(48 - 46.875) = 47.308 \\ &e_EST_k = (1 - 0.3846)1.878=1.54\\ ... \end{align*} k=1:k=2:...Kk=5+35=0.625x k=40+0.625(51−40)=46.875eESTk=(1−0.625)5=1.878Kk=1.878+31.878=0.3846x k=46.875+0.3846(48−46.875)=47.308eESTk=(1−0.3846)1.878=1.54
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
