
234 Optimal Estimation of Dynamic Systems
Table 4.2: Discrete-Time Linear Consider Kalman Filter
x
k+1
= Φ
k
x
k
+ Ψ
k
p+ Γ
k
u
k
+ ϒ
k
w
k
, w
k
∼ N(0, Q
k
)
Model
˜
y
k
= H
x
k
x
k
+ H
p
k
p+ v
k
, v
k
∼ N(0, R
k
)
ˆ
x(t
0
)=
ˆ
x
0
P
xx
0
= E
˜
x(t
0
)
˜
x
T
(t
0
)
Initialize
P
xp
0
= E
˜
x(t
0
)[
ˆ
p(t
0
) −p]
T
P
pp
0
= E
[
ˆ
p(t
0
) −p][
ˆ
p(t
0
) −p]
T
K
k
=
P
−
xx
k
H
T
x
k
+ P
−
xp
k
H
T
p
k
(H
x
k
P
−
xx
k
H
T
x
k
+ H
x
k
P
−
xp
k
H
T
p
k
Gain
+H
p
k
P
−
px
k
H
T
x
k
+ H
p
k
P
pp
k
H
T
p
k
+ R
k
)
−1
ˆ
x
+
k
=
ˆ
x
−
k
+ K
k
˜
y
k
−H
x
k
ˆ
x
−
k
−H
p
k
ˆ
p
−
k
ˆ
p
+
k
=
ˆ
p
−
k
Update
P
+
xx
k
=
I −K
k
H
x
k
P
−
xx
k
−K
k
H
p
k
P
−
px
k
P
+
xp
k
=
I −K
k
H
x
k
P
−
xp
k
−K
k
H
p
k
P
pp
k
ˆ
x
−
k+1
= Φ
k
ˆ
x
+
k
+ Ψ
k
ˆ
p
+
k
+ Γ
k
u
k
ˆ
p
−
k+1
=
ˆ
p
+
k
Propagate P
−
xx
k+1
= Φ
k
P
+
xx
k
Φ
T
k
+ Φ
k
P
+
xp
k
Ψ
T
k
+Ψ
k
P
+
px
k
Φ
T
k
+ Ψ
k
P
pp
k
Ψ
T
k
+ ϒ
k
Q
k
ϒ
T
k
P
−
xp
k+1
= Φ
k
P
+
xp
k
+ Ψ
k
P
pp
k
4.4.2 Consider Propagation Equations ...