Algorithm 3:

Particle Filter Algorithm for Tool Tracking

$$\begin{array}{l}Input:\text{Tool Model,}{\chi }_{t-1},u_t,z_t,P_t,g_{CB}^t\\ 1{\overline{\chi }}_t={\chi }_t=\varnothing \\ 2form=1:Mdo\\ 3\text{sample}{x}_t^m~p(x_t|u_t,x_{t-1}^m)\\ 4\text{Compute and normalize the Chamfer matching score using the set of virtual images:}\\ 5p(z_t|x_t^m)~virtual\_tool\_rendering(\text{Tool Model,}{x}_t^m,P_t,g_{CB}^t)\\ 6w_t^m=p(z_t|x_t^m)\\ 7{\overline{\chi }}_t={\overline{\chi }}_t+\langle x_t^m,w_t^m\rangle \\ 8end\\ 9form=1:Mdo\\ 10\text{draw}i\text{with probability}\propto w_t^i\\ 11\mathrm{add}{x}_t^i\text{to}{\chi }_t\\ 12end\\ Output:{\chi }_t\end{array}$$