procedure
SMPF (, {ΔTr,k}) |
Calculate based on {ΔTr,k} |
for
l ← 1:4 do
|
for
i ← (0, 0):(239, 179) do
|
if number of events in the i-th pixel < 3 then
|
ETi,k,l ← 0 |
end if
|
end for
|
end for
|
for
l ← 1:4 do
|
for
j ← 1:Ns
do
|
Draw
|
Get the particle's temporal evidence ETi,k,l according to
|
end for
|
end for
|
for
l ← 1:4 do
|
Nreselect ← Equation (5) |
if
Nreselect < Thl
then
|
Run reselection (replacement) |
Get the new particle set and the new temporal evidence set
|
for
j ← 1:Ns
do
|
|
end for
|
end if
|
Normalize
|
Calculate μTk,l and ΣTk,l of the temporal evidence matrix ETk,l
|
for
j ← 1:Ns
do
|
Get spatial evidence ESi,k,l according to ,
|
end for
|
Normalize
|
for
j ← 1:Ns
do
|
Update the particle's weight
|
end for
|
Normalize
|
Neff ← Equation (7) |
if
Neff < Theff × Ns
then
|
Run resampling |
Get the new particle set
|
for
j ← 1:Ns
do
|
|
end for
|
end if
|
Update the tracked position of the l-th LED marker,
|
for
j ← 1:Ns
do
|
|
end for
|
end for
|
end procedure
|