
294 Optimal Estimation of Dynamic Systems
where y
( j)
k
≡ h(x
( j)
1k
).
• Resample x
( j)
1k
if needed.
• Propagate the particles for each particle j = 1, 2,...,N
x
( j)
1k+1
∼ N(f(x
( j)
1k
)+Φ
1k
x
−( j)
2k
,Φ
1k
P
−
2k
Φ
T
1k
+ ϒ
1k
Q
1k
ϒ
T
1k
) (4.218)
• Compute the Kalman gain
K
k
= P
−
2k
Φ
T
1k
[Φ
T
1k
P
−
2k
Φ
T
1k
+ ϒ
1k
Q
1k
ϒ
T
1k
]
−1
(4.219)
• Update the Kalman filters for each particle j = 1, 2, ..., N
x
+( j)
2k
= x
−( j)
2k
+ K
k
x
( j)
1k+1
−f(x
( j)
1k
) −Φ
1k
x
−( j)
2k
(4.220a)
P
+
2k
=[I −K
k
Φ
1k
]P
−
2k
(4.220b)
• Propagate the Kalman filters for each particle j = 1, 2,...,N
x
−( j)
2k+1
= D
k
x
+( j)
2k
+C
k
x
( j)
1k+1
−f(x
( j)
1k
)
(4.221a)
P
−
2k+1
= D
k
P
+
2k
D
T
k
+ ϒ
2k
¯
Q
2k
ϒ
T
2k
(4.221b)
where
¯
Q
2k
= Q
2k
−Q
T
12k
Q
−1
1k
Q
12k
(4.222a)
C
k
= ϒ
2k
Q
T
12 ...