引入一个离散控制过程系统：
X
(
k
)
=
A
X
(
k
−
1
)
+
B
U
(
k
)
+
W
(
k
)
X_{(k)}=AX_{(k-1)}+BU_{(k)}+W_{(k)}
X(k)=AX(k−1)+BU(k)+W(k)
系统测量值：
Z
(
k
)
=
H
X
(
k
)
+
V
(
k
)
Z_{(k)}=HX_{(k)}+V_{(k)}
Z(k)=HX(k)+V(k)
卡尔曼5个核心公式：
基于上一时刻进行的计算：
(1)
系
统
状
态
值
:
X
(
k
∣
k
−
1
)
=
A
X
(
k
−
1
∣
k
−
1
)
+
B
U
(
k
)
系统状态值: X_{(k|k-1)} = AX_{(k-1|k-1)}+BU_{(k)} \tag{1}
系统状态值:X(k∣k−1)=AX(k−1∣k−1)+BU(k)(1)
(2) 系 统 协 方 差 : P ( k ∣ k − 1 ) = A P ( k − 1 ∣ k − 1 ) A ′ + Q 系统协方差: P_{(k|k-1)} = AP_{(k-1|k-1)}A'+Q \tag{2} 系统协方差:P(k∣k−1)=AP(k−1∣k−1)A′+Q(2)
然后对当前时刻进行更新：
(3) X ( k ∣ k ) = X ( k ∣ k − 1 ) + K g ( k ) ( Z ( k ) − H X ( k ∣ k − 1 ) ) X_{(k|k)}=X_{(k|k-1)}+Kg_{(k)}(Z_{(k)}-HX_{(k|k-1)}) \tag{3} X(k∣k)=X(k∣k−1)+Kg(k)(Z(k)−HX(k∣k−1))(3)
(4) 其 中 K g ( k ) = P ( k ∣ k − 1 ) H ’ H P ( k ∣ k − 1 ) H ’ + R 其中Kg_{(k)}=\frac{P_{(k|k-1)}H’}{HP_{(k|k-1)}H’+R} \tag{4} 其中Kg(k)=HP(k∣k−1)H’+RP(k∣k−1)H’(4)
(5) P ( k ∣ k ) = ( I − K g ( k ) H ) P ( k ∣ k − 1 ) P_{(k|k)}=(I-Kg_{(k)}H)P_{(k|k-1)} \tag{5} P(k∣k)=(I−Kg(k)H)P(k∣k−1)(5)
3万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
