Study on GPS/INS System Using Novel Filtering Methods for Vessel Attitude Determination

Algorithm 1

for from 1 to 𝑁₁

  for from 1 to 𝑁

    find particle 𝑋^𝑖_𝑘 with the biggest weight 𝑤^𝑖_𝑘 from 𝑁 particles */This is the best particle;

    find particle 𝑋^𝑗_𝑘 with the biggest weight 𝑤^𝑗_𝑘 from 𝑁 particles */This is the worst particle;

  end

  𝑋^∼𝑗_𝑘=𝑎∗𝑋^𝑖_𝑘+(1−𝑎)𝑋^𝑗_𝑘  */Replace the worst particle by the linear combination of 𝑋^𝑖_𝑘 and 𝑋^𝑗_𝑘, 0≤𝑎≤1;

  Re-calculate the weight of 𝑋^∼𝑗_𝑘 and then normalized it;

end