例: 测量硬币直径
有以下测量值
z
1
=
50.1
m
m
z_1=50.1mm
z1=50.1mm
z
2
=
50.4
m
m
z_2=50.4mm
z2=50.4mm
z
3
=
50.2
m
m
z_3=50.2mm
z3=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
=
1
k
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
x
^
k
−
1
+
1
k
z
k
⇒
x
^
k
−
1
=
x
^
k
−
1
+
1
k
(
z
k
−
x
^
k
−
1
)
x
^
k
−
1
=
x
^
k
−
1
+
k
k
(
z
k
−
x
^
k
−
1
)
\begin{aligned} \hat{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{1}{k}\frac{k-1}{k-1}(z_1+z_2+z_3+...+z_{k-1})+\frac{1}{k}z_k\\ &=\frac{k-1}{k}\hat{x}_{k-1}+\frac{1}{k}z_k\\ &=\hat{x}_{k-1}-\frac{1}{k}\hat{x}_{k-1}+\frac{1}{k}z_k\\ \Rightarrow \hat{x}_{k-1}=\hat{x}_{k-1}+\frac{1}{k}(z_k-\hat{x}_{k-1})\\ \hat{x}_{k-1}=\hat{x}_{k-1}+k_k(z_k-\hat{x}_{k-1}) \end{aligned}\\
x^k⇒x^k−1=x^k−1+k1(zk−x^k−1)x^k−1=x^k−1+kk(zk−x^k−1)=k1(z1+z2+z3+...+zk)=k1(z1+z2+z3+...+zk−1)+k1zk=k1k−1k−1(z1+z2+z3+...+zk−1)+k1zk=kk−1x^k−1+k1zk=x^k−1−k1x^k−1+k1zk
当前估计值=上一次估计值+系数 x（当前测量值 - 上一次测量值）
算法步骤
step1:
计算
K
a
l
m
a
n
G
a
i
n
k
k
=
e
E
S
T
k
−
1
e
E
S
T
k
−
1
+
e
M
E
A
k
KalmanGain\quad k_k=\frac{e_{EST_{k-1}}}{e_{EST_{k-1}}+e_{MEA_k}}
KalmanGainkk=eESTk−1+eMEAkeESTk−1
step2:
计算
x
^
k
=
x
^
k
−
1
+
k
k
(
z
k
−
x
^
k
−
1
)
\hat{x}_k=\hat{x}_{k-1}+k_k(z_k-\hat{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
3291
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
