# Position and orientation algorithm

The P&O algorithm forms the computational core of the Anser EM tracking systm. The algorithm utilises the demodulated waveform data to resolve the position and angular orientation of the tracking sensor coil. It does this by minimising the error between the waveform data and the magnetic field model of the system using non-linear least-quares solving methods.

The algorithm is composed of two stages:

• Calibration. This procedure tweaks the magnetic field model of the system using a set of predefined test points. A vector of magnetic field calibration constants are produced with each constant corresponding to a specific emitter coil in the field generator.
• Solver The solver utilises the calibration constants to resolve sensor positions and orientations during system operation. The received magnetic field strengths are compared to those of the system magnetic field model. The solution vector of ${\displaystyle (x,y,z,\theta ,\phi )}$ is adjusted by the solver to minimise the error between the received signal and the model. The vector of ${\displaystyle (x,y,z,\theta ,\phi )}$ which minimises this error represents the position and orientation of the sensor.

These two steps are discussed in detail in the following sections.

Block diagram of calibration routine

## Calibration

This section describes the calibration algorithm used in the Anser EMT system. The approach utilises a nonlinear least squares technique to minimise the error between the magnetic field measurements (from the sensor) and the numerical magnetic field model. Calibration is a necessary procedure since certain system parameters cannot be known to required precisions solely through the system design. Parameters such as amplifier gain and filter performance cannot be directly encapsulated within the magnetic filament model described in section 3a. The result is that each emitter coil produces a time-varying magnetic field, the magnitude of which cannot be determined precisely through analytical means.

Calibration allows for experimental refinement of the magnetic field model for each emitter coil. This is performed by acquiring a set of testpoints at known locations relative to each coil. The procedure yields a scaling factor for each emitter coil model which accurately encapsulates the otherwise difficult to determine circuit parameters such as amplifier gain and filter attenuation.

The resulting scaling factors fit the current filament model of the coils to the induced voltage signals received by the sensor coil.

Consider the magnetic flux measurements at ${\displaystyle m}$ testpoints from the ${\displaystyle i_{th}}$ coil in the FG to be represented by ${\displaystyle \Phi _{meas}^{i}}$, and the associated modelled flux at this point too be given by ${\displaystyle \Phi _{calc}^{i}}$. The objective function to be minimised the error between the two fluxs can by written as:

${\displaystyle F_{i}(\kappa _{i})=\sum _{j=1}^{m}\left(\Phi _{meas}^{i}(x_{j},y_{j},z_{j},\theta _{j},\varphi _{j})-\kappa _{i}\Phi _{calc}^{i}(x_{j},y_{j},z_{j},\theta _{j},\varphi _{j})\right)^{2}}$

where ${\displaystyle \kappa _{i}}$ is the calibration scaling factor for a particular coil, ${\displaystyle j}$ is the current test point, ${\displaystyle (x_{j},y_{j},z_{j},\theta _{j},\varphi _{j})}$ is the location and orientation of the sensor at testpoint ${\displaystyle j}$ and ${\displaystyle m}$ is the total number of testpoints. The scaling factors ${\displaystyle \kappa _{i}}$ scale the magnetic field models of each transmitter coil. These scaling factors are required because the amplitude of each generated magnetic field varies due to the circuitry powering each coil.

![enter image description here][1]

The calibration procedure of the Anser system requires 49 testpoints in a 7x7 grid on a Duplo baseplate. The procedure takes ~3 mins and can be viewed **here**

### Z axis offset

The nature of the calibration leads to some uncertainty in the z-axis position of the sensor at each test point. This is because the sensor is mounted axially in vertical position inside a Duplo housing in which the uncertainty can be within 1-2mm. A second calibration parameter is introduced into the calibration such the objective function is:

${\displaystyle F_{i}(\kappa _{i},\mathbf {r_{i}^{offset}} )=\sum _{j=1}^{m}\left(\Phi _{meas}^{i}(\mathbf {r_{j}} -\mathbf {r_{i}^{offset}} ,\theta _{j},\varphi _{j})-\kappa _{i}\Phi _{calc}^{i}(\mathbf {r_{j}} -\mathbf {r_{i}^{offset}} ,\theta _{j},\varphi _{j})\right)^{2}}$

where the cartesian coordinates of the point are given by

${\displaystyle \mathbf {r_{j}} =(x_{j},y_{j},z_{j})}$
and the z-axis offset is given as
${\displaystyle \mathbf {r_{i}^{offset}} =(0,0,z_{i}^{offset})}$

The ${\displaystyle z_{i}^{offset}}$ indicates the calculated error in the ${\displaystyle z}$ direction of the system. The sensor position can be readjusted in order to neutralise the offset value before another calibration is performed.

### Minimisation

The minimisation process aims to minimise each ${\displaystyle F_{i}}$. Once complete, the resulting value of ${\displaystyle \kappa _{i}}$ is the calibration factor for the ${\displaystyle i^{th}}$ coil. A vector of 8 calibration values is produced for the Anser EMT system. Details of the minimisation process can be read in the comments of the Anser Matlab code. The calibration procedure and instructions regarding testpoint acquisition can be found in the system manual.

## Position and orientation solver

The P&0 solver takes the demodulated sensor data and attempts to fit the signal strengths to the calibrated magnetic field model. The vector of values ${\displaystyle (x,y,z,\theta ,\varphi )}$ which minimise the error between the measurements and model are the position and orientation of the sensor.

The P&O solver can be thought of as a dual to the calibration procedure. During the calibration the magnetic field model it fit using a set of **known** ${\displaystyle (x,y,z,\theta ,\varphi )}$ positions to produce a set of scaling factors. The P&O solver utilises these scaling factors solve for a set of **unknown** test points in the field of operation.

The block diagram below shows a high level view of the solving process. Starting from an initial guess position and orientation ${\displaystyle (0,0,z_{init},0,0)}$ the nonlinear least squares algorithm compares the modelled flux ${\displaystyle \Phi }$ from coil ${\displaystyle i}$ (of ${\displaystyle m}$ total coils) with that of the real sensor. The algorithm iteratively modifies the initial guess until the difference between the ${\displaystyle \Phi _{calc}}$ and ${\displaystyle \Phi _{meas}}$ is minimised. The ${\displaystyle (x,y,z,\theta ,\varphi )}$ that simulataneously minimises the ${\displaystyle \Phi }$ error for all ${\displaystyle m}$ emitter coils is the calculated resolved sensor position.

Block diagram of solver operation

### Cost function

In order for minimisation to take place on must provide an objective function to the solving algorithm. The objective function to be minimised looks similar to the calibration objective function: a sum of squared magnetic flux errors due to each. For a particular sensor acquisition at an unknown point in space ${\displaystyle j}$, the objective function to minimise is given by:

${\displaystyle F(x,y,z,\theta ,\varphi )=\sum _{i=1}^{8}(\Phi _{meas}^{i}-\kappa _{i}\Phi _{calc}^{i}(x,y,z,\theta ,\varphi ))^{2}}$

where ${\displaystyle i}$ is the index of the emitter coil under consideration, ${\displaystyle Phi_{meas}^{i}}$ is the flux contribution from emitter coil ${\displaystyle i}$ and ${\displaystyle \Phi _{calc}^{i}}$ is the calculated flux contribution from coil ${\displaystyle i}$ at position ${\displaystyle (x,y,z,\theta ,\varphi )}$. In this objective function, the vector ${\displaystyle (x,y,z,\theta ,\varphi )}$ is the variable. The nonlinear solver in Matlab iteratively tweaks this vector in order to minimise the sum of the magnetic field errors due to each coil. The resulting sum of these errors is the cost function ${\displaystyle F}$. The vector of ${\displaystyle (x,y,z,\theta ,\varphi )}$ which results in the minimisation of ${\displaystyle F}$ is the position and orientation of the sensor at the point ${\displaystyle j}$, ${\displaystyle (x_{j},y_{j},z_{j},\theta _{j},\varphi _{j})}$

Details of the implementation, initial conditions and solver parameters can be read in the Matlab code for the P&O algorithm.