17 Modified Continuous-Time Particle Filter Algorithm …
249
1. Specify M, the number of auxiliary trajectories needed to simulate, h, the integration step. Draw the sample for initial state vectors X 0 and X
i
0 from the given
distribution with the probability density function ϕ 0 (x), i = 1, 2, . . . , M. Set
k = 0, Y 0 = 0, ω
i
0 = 1, i = 1, 2, . . . , M.
2. Set
M k =
M
i=1
ω
i
k (M 0 = M).
For the sample X k = {X
i
k }
M
i=1 with the set of associated weights W k = {ω
i
k }
M
i=1
find the following statistics: the estimate of the state vector (the unbiased MMSE
estimate)
ˆ
X k =
1
M k
M
i=1
ω
i
k X
i
k
and the estimate of the posterior covariance matrix
ˆ
R k =
1
M k
M
i=1
ω
i
k (X
i
k − ˆ
X k )(X
i
k − ˆ
X k )
T
.
Verify the condition T − t k = 0. If it is met, then stop. Obtain a realization of the
estimated state vector and the corresponding measurement vector at t k+1 :
X k+1 = F(t k , X k , h), Y k+1 = C(t k , X k , Y k , h), Z k =
Y k+1 − Y k
h
.
3. Obtain a realization of the state vector at t k+1 and update the corresponding
weight:
X
i
k+1 = F(t k , X
i
k , h),
ω
i
k+1 = ω
i
k e
μ(t k ,X
i
k ,Z k )h
,
where
μ(t, x, z) = c
T
(t, x)q(t)
z −
1
2
c(t, x)
,
q(t) = η
−1
(t), η(t) = ζ(t)ζ
T
(t).