Gating Techniques for Rao-Blackwellized Monte Carlo Data Association Filter

Algorithm 1

RBMCDA using gating techniques.

For  𝑘=1  to 𝑛  do

   For 𝑖=1 to 𝑁_𝑝 do

   For 𝑗=1 to 𝑇 do

     𝐱^(𝑗)_{𝑖,𝑘∣𝑘−1}=𝐅^(𝑗)_𝑘−1𝐱^(𝑗)_{𝑖,𝑘−1∣𝑘−1},                                  (i)

     𝐏^(𝑗)_{𝑖,𝑘∣𝑘−1}=𝐅^(𝑗)_𝑘−1𝐏^(𝑗)_{𝑖,𝑘−1∣𝑘−1}𝐅^𝑗_𝑘−1^′+𝐐^(𝑗)_𝑘−1.                             (ii)

   End for 𝑗

   End for 𝑖

   For 𝑗=1 to 𝑇 do

    𝐳̂^(𝑗)_{𝑘∣𝑘−1}=𝐇_𝑘^(𝑗)∑^𝑁𝑝_𝑖=1𝐰_{𝑖,𝑘−1}𝐱^(𝑗)_{𝑖,𝑘∣𝑘−1},                                  (iii)

    𝐒_𝑘^(𝑗)=𝐇_𝑘^(𝑗)∑^𝑁𝑝_𝑖=1𝐰_{𝑖,𝑘−1}𝐏^(𝑗)_{𝑖,𝑘∣𝑘−1}𝐇_𝑘^(𝑗)′+𝐑_𝑘^(𝑗),                            (iv)

    where 𝐳̂^(𝑗)_{𝑘∣𝑘−1} and 𝐒_𝑘^(𝑗) are the predicted measurement and innovation, respectively.

    For 𝑚=1 to 𝑁_{𝐦𝐞𝐚𝐬𝐮𝐫𝐞𝐦𝐞𝐧𝐭𝐬} (number of the measurements) do

      Choose 𝐳_𝑘(𝑚) that satisfies the following condition as the validated measurement of the 𝑗th target:

      𝐳_𝑘(𝑚)−𝐳̂^(𝑗)_{𝑘∣𝑘−1}^′𝐒_𝑘^(𝑗)−1𝐳_𝑘(𝑚)−𝐳̂^(𝑗)_{𝑘∣𝑘−1}≤𝛾.                           (v)

    End for 𝑚

   End for 𝑗

   For  𝑟=1 to 𝑁_{𝐒𝐕𝐑𝐬} (number of the SVRs) do

     For 𝑗=1 to 𝑇

      Calculate 𝑃_𝑒^(𝑗)_𝑟 using gating techniques.

     End for 𝑗

     For 𝑚=1 to 𝑁_𝑟

      For 𝑖=1 to 𝑁_𝑝

        Calculate data association priors 𝑝(𝑐_𝑖,𝑘∣𝑐_{𝑖,𝑘−𝑚∶𝑘−1}) by (8) based on the Markov chain

        model described in [1].

        For 𝑗=1 to 𝑇 do

           𝐳̂^(𝑗)_𝑖,𝑘=𝐇_𝑘^(𝑗)𝐱^(𝑗)_{𝑖,𝑘∣𝑘−1},                               (vi)

          𝐒^(𝑗)_𝑖,𝑘=𝐇_𝑘^(𝑗)𝐏^(𝑗)_{𝑖,𝑘∣𝑘−1}𝐇_𝑘^(𝑗)′+𝐑_𝑘^(𝑗).                            (vii)

          Calculate the marginal measurement likelihood

          𝑝(𝐳_𝑘∣𝑐_𝑖,𝑘=𝑗,𝑐_{𝑖,1∶𝑘−1},𝐳_1∶𝑘−1)=𝒩(𝐳_𝑘(𝑚)∣𝐳̂^(𝑗)_𝑖,𝑘,𝐒^(𝑗)_𝑖,𝑘).                   (viii)

        End for 𝑗

        Calculate the clutter likelihood

        𝑝(𝐳_𝑘∣𝑐_𝑖,𝑘=0,𝑐_{𝑖,1∶𝑘−1},𝐳_1∶𝑘−1)=𝑉⁻¹.                             (ix)

        Calculate the posterior distribution of 𝑐_𝑖,𝑘

        𝑝(𝑐_𝑖,𝑘∣𝑐_{𝑖,1∶𝑘−1},𝐳_1∶𝑘)=𝑝(𝐳_𝑘∣𝑐_𝑖,𝑘,𝑐_{𝑖,1∶𝑘−1},𝐳_1∶𝑘−1)𝑝(𝑐_𝑖,𝑘∣𝑐_{𝑖,𝑘−𝑚∶𝑘−1}).                  (x)

        Sample an association indicator 𝑐_𝑖,𝑘=𝑗 with probability 𝜌_𝑖^(𝑗)

        𝜌_𝑖^(𝑗)=𝜌_𝑖^(𝑗)∑^𝑇_𝑗′=0𝜌^(𝑗′)_𝑖, 𝑗=0,…,𝑇,                           (xi)

        where

        𝜌_𝑖⁽⁰⁾=𝑝(𝑐_𝑖,𝑘=0∣𝑐_{𝑖,1∶𝑘−1},𝐳_1∶𝑘),                            (xii)

        𝜌_𝑖^(𝑗)=𝑝(𝑐_𝑖,𝑘=𝑗∣𝑐_{𝑖,1∶𝑘−1},𝐳_1∶𝑘).                              (xiii)

        If 𝑗≠0 , update the 𝑗 th target using the measurement 𝐳_𝑘(𝑚)

          𝐊_𝑖,𝑘=𝐏^(𝑗)_{𝑖,𝑘∣𝑘−1}𝐇_𝑘^(𝑗)′𝐒^(𝑗)_𝑖,𝑘,                              (xiv)

          𝐱^(𝑗)_{𝑖,𝑘∣𝑘}=𝐱^(𝑗)_{𝑖,𝑘∣𝑘−1}+𝐊_𝑖,𝑘𝐳_𝑘(𝑚)−𝐳̂^(𝑗)_𝑖,𝑘,                        (xv)

          𝐏^(𝑗)_{𝑖,𝑘∣𝑘}=𝐏^(𝑗)_{𝑖,𝑘∣𝑘−1}−𝐊_𝑖,𝑘𝐒^(𝑗)_𝑖,𝑘𝐊_𝑖,𝑘^′.                           (xvi)

        End if

        Calculate the new weight

        𝑤_𝑖,𝑘∝𝑤_{𝑖,𝑘−1}𝑝(𝐳_𝑘∣𝑐_𝑖,𝑘,𝑐_{𝑖,1∶𝑘−1},𝐳_1∶𝑘−1)𝑝(𝑐_𝑖,𝑘∣𝑐_{𝑖,𝑘−𝑚∶𝑘−1})𝑝(𝑐_𝑖,𝑘∣𝑐_{𝑖,1∶𝑘−1},𝐳_1∶𝑘).                   (xvii)

      End for 𝑖

      Resample the particles if needed.

   End for 𝑚

   Calculate the target state vectors

   For 𝑗=1 to

      𝐱^(𝑗)_𝑘∣𝑘=∑^𝑁𝑝_𝑖=1𝑤_𝑖,𝑘𝐱^(𝑗)_{𝑖,𝑘∣𝑘}.                               (xviii)

   End for 𝑗

   End for 𝑟

End for 𝑘