Trajectory Generation Model-Based IMM Tracking for Safe Driving in Intersection Scenario

Algorithm 1

For i1 from a−Δa to a  +  Δa do

 For i2 from b−Δb to b  +  Δb do

  For i3 from c−Δc to c  +  Δc do

      For i4 from s−Δs to s  +  Δs do

      Calculate equations of (2) and the sum of

      𝐹(𝑥,𝑝,𝑡) in (11).

      End for

  End for

 End for

End for

Compare the set of 𝐹(𝑥,𝑝,𝑡), and the trajectory which

has the smallest 𝐹(𝑥,𝑝,𝑡) should be picked out as the

optimal trajectory.