calculate distance with only the first m_min particles

src/tree.jl

@@ -223,25 +223,28 @@ end
 
 function gen_packing!(D::AdaOPSTree, S, O, belief::WeightedParticleBelief, a, p::AdaOPSPlanner)
     sol = solver(p)
-    m = length(S)
+    m_min = sol.m_min
+    m_max = length(S)
     w = weights(belief)
 
     next_obs = 1 # denote the index of the next observation branch
     for i in eachindex(O)
-        w′ = resize!(D.weights[D.b+next_obs], m)
+        w′ = resize!(D.weights[D.b+next_obs], m_min)
         o = O[i]
-        reweight!(w′, w, S, a, o, p.pomdp)
+        # reweight first m_min particles
+        reweight!(w′, view(w, 1:m_min), view(S, 1:m_min), a, o, p.pomdp)
         # check if the observation is already covered by the packing
-        w′ .= w′ ./ sum(w′)
-        obs_ind = in_packing(w′, p.w, sol.delta)
-        if obs_ind != 0
+        norm_w = p.norm_w[next_obs]
+        norm_w .= w′ ./ sum(w′)
+        obs_ind = in_packing(norm_w, view(p.norm_w, 1:(next_obs-1)), sol.delta)
+        if obs_ind !== 0
             # merge the new obs into an existing obs
             p.obs_w[obs_ind] += p.obs_w[i]
         else
             # add the new obs into the packing
             p.obs_w[next_obs] = p.obs_w[i]
             O[next_obs] = o
-            push!(p.w, w′)
+            push!(p.w, resize!(w′, m_max))
             next_obs += 1
         end
     end
@@ -250,6 +253,10 @@ function gen_packing!(D::AdaOPSTree, S, O, belief::WeightedParticleBelief, a, p:
     resize!(O, n_obs)
     resize!(p.obs_w, n_obs)
 
+    for i in eachindex(p.w)
+        reweight!(view(p.w[i], (m_min+1):m_max), view(w, (m_min+1):m_max), view(S, (m_min+1):m_max), a, O[i], p.pomdp)
+    end
+
     return nothing
 end