\chapter{Algorithms for source localization}

This charter introduces several algorithms that can be utilized for autonomous localization of gamma radiation sources using technical equipment described earlier. Some of them are not dependent on an arrangement of a~detection system, others rely on directional sensitivity provided by the proposed two-detector system. All algorithms use total count for quantification of radiation intensity.

\section{Mapping algorithm}\label{sec.mapping}

Algorithm termed as mapping is an elementary algorithm for measurement of environmental quantities in the area of interest. Mapping is described in several papers, e. g. \cite{ares}, \cite{airborne} and \cite{tom-paper}. The idea is to pass the whole area along parallel equidistant lines and measure the desired quantity periodically. If the line spacing and the robot’s velocity are small enough, even subtle changes in the measured quantity can be noticed. In terms of radiation source localization – even weak sources can be found. That is apparently an advantage of the mapping. The disadvantage is that time requirements increase rapidly with size of the measured area. Schematic example of mapping trajectory in pentagonal area is shown in Fig.~\ref{fig.maptraj}.

\begin{figure}[!h]
  \begin{center}
    \includegraphics[scale=0.75]{images/maptraj.pdf}
  \end{center}
  \caption{Schematic of the mapping trajectory}
  \label{fig.maptraj}
\end{figure}

Node {\it meas-trajectory} has to be added to the configuration file in order to set parameters for mapping. The measured area is defined by node {\it polygon} which contains a~sequence of coordinates of polygon’s vertices. The minimal count of vertices is three for triangular area. It is assumed that the sequence is ordered. Those coordinates are complemented by coordinates of the base station. Another parameter is line spacing. Optimal value of spacing $d$ can be chosen based on the parameters of the weakest source to be found. In the worst case, the source is located exactly in the middle between two mapping lines. Dose rate generated by the source measured on the line should be at least three~times higher than the background. Typical background rate is 0.17~\usvh (the value has been measured in designed experimental site), and therefore the source should generate approximately 0.5~\usvh in distance $\frac{d}{2}$. The following formula can be deduced:

\begin{equation}
d = 2 \sqrt{\frac{\dot{D}_1}{0.5 \cdot 10^{-6}}} \myunit{m; \usvhm}
\end{equation}
where $\dot{D}_1$ stands for dose rate generated by the weakest searched source in a~distance of 1 meter -- can be calculated from \ref{eq.drate}. For example, if cobalt-60 ($^{60}$Co) with activity 215 kBq should be found, $\dot{D}_1$ is equal to 79.6 \nsvh, an therefore the spacing parameter should be 0.8 m.

The last parameter to be set determines the spacing of generated waypoints. It should be a~value ranging from 0.2 to 1.0 meters.

\subsection{Generation of waypoints}

At first, waypoints are generated in local Cartesian coordinate system. Its x-axis is parallel to cirles of latitude, east is the positive direction. The y-axis is parallel to meridians, north is the positive direction. Points in WGS-84 have coordinates ($\phi, \lambda$) where $\phi$ is latitude and $\lambda$ is longitude. Point in the local system have coordinates ($x, y$). The z-coordinate is omitted because the problem is solved in plane.

The proposed transformation is based on rhumb lines \cite{movable}. Rhumb line is a~path of constant bearing which crosses all meridians at the same angle and is a~straight line in Mercator projection. Coordinates of the base station in WGS-84 are ($\phi_0, \lambda_0$) and (0, 0) in the local system. The transformed point have coordinates ($\phi_1, \lambda_1$) in WGS-84 and ($x_1, y_1$) in the local system. When coordinates are transformed from WGS-84 to the local system, the x-coordinate is rhumb-line distance between base station and point at same latitude.

\begin{equation}
x_1 = \cos \phi_0 \cdot (\lambda_1 - \lambda_0) \cdot R \myunit{m; rad, rad, rad, m}
\end{equation}
where $R$ is Earth’s radius and is equal to 6,371,008 m \cite{earth}. Similarly the y-coordinate is rhumb-line distance between base station and point at the same longitude.

\begin{equation}
y_1 = (\phi_1 - \phi_0) \cdot R \myunit{m; rad, rad, m}
\end{equation}

When coordinates are transformed from the local system to WGS-84, coordinates are equal to coordinates of the destination point along rhumb line given distance and bearing from base station. Longitude is given as:

\begin{equation}
\lambda_1 = \lambda_0 + \frac{x_1}{R \cdot \cos \phi_0} \myunit{rad; rad, m, m, rad}
\end{equation}

And latitude is given as:

\begin{equation}
\phi_1 = \phi_0 + \frac{y_1}{R} \myunit{rad; rad, m, m}
\end{equation}

The goal is to find equations of parallel lines with set spacing, then find their intersections with polygon, and finally split line segments defined by the intersections into waypoints and order them. Coordinates of vertices are given in WGS-84 and have to be transformed first. Then it is possible to find equations of polygon’s edges. Fundamental principles of the analytic geometry are taken from \cite{geometry}. A majority of the following equations is deduced by the author of this thesis.

All lines in this thesis are described by a~general form of equation of a~line, that is: $ax + by + c = 0$, where \vect{a,b} is a~normal vector of the line. Given two vertices \pnt{x_1,y_1} and \pnt{x_2,y_2}, the corresponding edge is described as:

\begin{subequations}
\begin{align}
a&= y_1 - y_2\\
b&= x_2 - x_1\\
c&= -ax_1 - by_1 = -ax_2 - by_2
\end{align}
\end{subequations}

The edge defined by first two vertices is denoted as {\it base line}. The first vertex represent a~point where the measurement trajectory begins. The next step is to find the vertex that is farthest from the base line. Distance of point \pnt{x_1,y_1} from line can be calculated by the following formula:

\begin{equation}
d = \frac{|ax_1 + by_1 + c|}{\sqrt{a^2 + b^2}}
\end{equation}

The last parallel line should pass through this farthest vertex. If the distance is not integer multiple of desired line spacing, there are two options. The distance of some two lines will differ from others, or all lines will be equidistant with slightly different spacing. The latter option is used here. A line perpendicular to base line given by \vect{a_1, b_1, c_1} passing through point \pnt{x_1,y_1} is found as:

\begin{subequations}\label{eq.perpendicular}
\begin{align}
a&= b_1\\
b&= -a_1\\
c&= a_1y_1 - b_1x_1
\end{align}
\end{subequations}

The intersection of lines given by \vect{a_1, b_1, c_1} and \vect{a_2, b_2, c_2} has the following coordinates:

\begin{subequations}\label{eq.intersections}
\begin{align}
y&= \frac{c_1a_2 - c_2a_1}{b_2a_1 - b_1a_2}\label{eq.intersection1}\\
x&= \frac{-c_1 -b_1y}{a_1}
\end{align}
\end{subequations}

If the denominator in Eq.~\ref{eq.intersection1} is equal to zero, intersection does not exist. In this case, it has to exist because the lines are perpendicular. The line segment between points \pnt{x_1,y_1} and \pnt{x_2,y_2} can be easily splitted into $n$ equidistant points. If the length of the line segment is $d$ and desired point spacing is $s$, the number of points is:

\begin{equation}\label{eq.pntcount}
n = \floor*{\frac{d}{s}} + 1
\end{equation}

Coordinates of $i$-th point where $0 \leq i < n$ are given by these equations:

\begin{subequations}\label{eq.pntcoords}
\begin{align}
x_i&= x_1 + i \frac{x_2-x_1}{n-1}\\
y_i&= y_1 + i \frac{y_2-y_1}{n-1}
\end{align}
\end{subequations}

Then perpendicular lines passing through each of these points using Eq.~\ref{eq.perpendicular}. These lines are parallel to the base line, and are equidistant. In order to find intersections of the line and polygon, it is necessary to examine if intersections exist for each edge. The edge is a~line segment described by line equation and two distint end points \pnt{x_1,y_1} and \pnt{x_2,y_2}. Intersection with the line \pnt{x,y} can be gained from Eq.~\ref{eq.intersections}. Intersection with the line segment exists under the following condition:

\begin{equation}\label{eq.polygonint}
x_1 \leq x \leq x_2 \land y_1 \leq y \leq y_2
\end{equation}
assuming $x_1 < x_2 \land y_1 < y_2$. The problem is solved analogically for other relations between end points. Knowing the intersections, parallel line segments may be splitted to sequences of waypoints with desired spacing according to Eq.~\ref{eq.pntcount} and Eq.~\ref{eq.pntcoords}. The waypoints have to be ordered in a~way so that neighboring sequences of waypoints have opposite directions. Finally, the waypoints are transformed to WGS-84.

Proposed algorithm for generation of waypoints is generally applicable for all convex polygons. It can be used also for some concave polygon when all lines parallel to the base line have maximally two intersections with the polygon. Therefore, it depends not only on the shape of the polygon but also on choice of the base line.

\subsection{Interpolation of the source position}\label{sec.interp}

Set of scattered points is a~result of the mapping. Those are not very suitable for visualization and further map processing, e.g. conversion to 3D point cloud. Therefore, calculation of radiation intensity (either total count or dose rate) in points in regular grid is needed. This can be done for example through Delaunay triangulation \cite{delaunay}. This interpolation is implemented in \texttt{scatteredInterpolant} class in MATLAB. One of three interpolation methods may be specified -- either linear, nearest neighbor or natural neighbor interpolation. The linear one is used in this thesis. After performing interpolation, the data can be visualized and position of sources can be marked manually.

Automatic computation of the position consists of three steps. First, local maxima need to be found. To eliminate false positive detections, custom 2D peak detector is used and tuned with data from real experiments. Basically, each point from data set is compared to its neighborhood in a~manner shown in Fig.~\ref{fig.peakdet}, and its absolute value is compared to a~threshold. Distance between cells is 0.04 m. Value of parameter {\it k} is chosen in range from 0.95 to 1.00, parameter {\it Th} is set to double of the lowest measured intensity.

\begin{figure}[!h]
  \begin{center}
    \includegraphics[scale=0.36]{images/peak_det.pdf}
  \end{center}
  \caption{A scheme of kernel for peak detection}
  \label{fig.peakdet}
\end{figure}

Afterwards, measured data points within defined radius around each maximum are selected for further processing. These points are then fitted with suitable function. Theoretical dependency of measured radiation intensity on distance from source located in the \pnt{x_s,y_s} is described as:

\begin{equation}\label{eq.intensity}
I = \frac{K}{(x-x_s)^2 + (y-y_s)^2 + h^2}
\end{equation}
where $K$ is constant and $h$ represents height of the detector above surface. In real measurement the function is distorted by dead time and other effects. It is possible to use paraboloid (quadratic function) as a~rough approximation because the fitting can be easily performed analytically, using least squares method \cite{ls-paraboloid}. The approximation in 2D is illustrated in Fig.~\ref{fig.datafit}. The similarity of both function should be sufficient to provide satisfactory result within limited radius.

\begin{figure}[!h]
  \begin{center}
    \includegraphics[scale=0.75]{images/datafit.pdf}
  \end{center}
  \caption{A comparison with fitting function}
  \label{fig.datafit}
\end{figure}

Paraboloid is given by the following formula \cite{paraboloid}:

\begin{equation}
\label{eq.paraboloid}
z = f(x, y) = p_1 + p_2x + p_3y + p_4x^2 + p_5xy + p_6y^2
\end{equation}

With set of points \pnt{x,y,z} where $z$ is intensity parameters of paraboloid can be calculated by solving matrix equation \cite{paraboloid}:

\begin{equation}
p =
\left \lbrack
	\begin{tabular}{c}
		$p_1$ \\
		$p_2$ \\
		$p_3$ \\
		$p_4$ \\
		$p_5$ \\
		$p_6$
	\end{tabular}
\right \rbrack
=
\left \lbrack
	\begin{tabular}{cccccc}
		$\bm{1}$ & $\bm{x}$ & $\bm{y}$ & $\bm{x^2}$ & $\bm{xy}$ & $\bm{y^2}$
	\end{tabular}
\right \rbrack
\bm{^{-1} z}
\end{equation}
where components of the matrix on the right side are column vectors composed of points' coordinates. The condition for extreme is where partial derivatives with respect both to $x$ and $y$ are equal to 0.

\begin{equation}
\frac{\partial z}{\partial x} = \frac{\partial z}{\partial y} = 0
\end{equation}

From these equations it is possible to deduce formulas for computing $x$ and $y$ coordinates of the maximum of paraboloid described by equation \ref{eq.paraboloid}. They also correspond with coordinates of the searched source.

\begin{equation}
x_{max} = \frac{p_3p_5-2p_2p_6}{4p_4p_6-p_5^2}
\end{equation}

\begin{equation}
y_{max} = \frac{p_2p_5-2p_3p_4}{4p_4p_6-p_5^2}
\end{equation}

\section{Strong source search algorithm}\label{sec.strong}

In situation when finding only one source is required and timing is important following algorithm may be used. It is based on dynamic change of trajectory in accordance with measured data. It is assumed that the source is strong enough to be detectable in considerable part of the scanned area.

First, the same trajectory as in case of map building is planned. After the robot reaches end of a~line the data are searched for peak in radiation intensity. Peak is defined as data point which has higher total count than its neighbouring points and also $k$ times higher total count than the line's minimum (peak has to exceed radiation background). If peaks are found in two neighbouring lines and their positions correspond, then the trajectory is altered and new waypoints are generated in direction perpendicular to current line passing through center of peak projections to this line. A projection is given by intersection (\ref{eq.intersections}) of the line which it is projected to and a~perpendicular line (\ref{eq.perpendicular}) passing through the peak. The robot follows the line in the new direction denoted as {\it normal line} until another peak in measured radiation intensity occurs.

Afterwards, final sequence of waypoints denoted as {\it loop} is generated. The purpose of a~loop is to get data for more precise interpolation of the source position. That is achieved if data lines form approximately right angle and have intersection close to the source. Waypoints for a~loop are generated within radius that can be set through configuration file. The radius should correspond to an interpolation radius. A schematic example of the measurement trajectory is shown in Fig.~\ref{fig.strongtraj}

\begin{figure}[!h]
  \begin{center}
    \includegraphics[scale=0.75]{images/strongtraj.pdf}
  \end{center}
  \caption{Schematic of the strong source search trajectory}
  \label{fig.strongtraj}
\end{figure}

\section{Characteristics of a~direction-sensitive system}\label{sec.dirsens}

To verify directional sensitivity of proposed two-detector system, following experiments were set. In the first experiment the detectors were placed in vertical position with 8 cm distance of its centers. As a~radiation source $^{60}$Co with activity approximately 50~kBq was used. Such a~weak source was picked in order to measure multiple characteristics in the laboratory safely. The experiment was performed with the source located in angular distances from 0° to 330° with 30° step and in 17.5 cm distance from center of the system. All measurements lasted 40 s with sampling period 1~s.

Angular dependence of the ratio of second detector readings to first detector reading can be found in picture \ref{fig.ratiodep}. Two curves are plotted there – the orange one represents measurement during which 4 mm thick lead plate was present between detectors. Usage of shielding is inspired by \cite{shielding}. The spread of values in the worst case scenario for shielded option is also charted. The shape of a~function resembles the theoretical one satisfyingly. The directional sensitivity is better with shielding, it is therefore used in further experiments.

\begin{figure}[!h]
  \begin{center}
    \includegraphics[scale=0.8]{images/dirratio.pdf}
  \end{center}
  \caption{Angular dependence of the ratio of values measured by detectors}
  \label{fig.ratiodep}
\end{figure}

The second experiment was performed with detectors mounted on the robot. The purpose was to quantify how much radiation is attenuated by the robot’s body. A stronger source was used – caesium-137 ($^{137}$Cs) with activity approximately 1~MBq. The source was located in angular distances from 0° to 350° with 10° step and in 120 cm distance from the first, and afterwards also from the second detector. Again, all measurements lasted 40~s with sampling period 1~s.

Directional characteristic of the used radiation detection system was evaluated. It is shown in polar plot in Fig.~\ref{fig.dirchar}. The robot's forward direction is denoted as 0°, number of mean measured counts relative to forward direction is at radial axis in percent. Thanks to system geometry and shielding lead layer, both detectors are differently sensitive to radiation coming in different incident angles. The radiation coming from angles around 180° (through the robot) is notably attenuated.

\begin{figure}[!h]
  \begin{center}
    \includegraphics[scale=0.7]{images/dirchar.pdf}
  \end{center}
  \caption{Directional characteristics of the used detectors}
  \label{fig.dirchar}  
\end{figure}

\section{Circular algorithms}

When a~robot rides through a~closed loop, all possible angles between detectors and radiation sources are taken. The resulting data set has greater information value about position of sources than single measurement. The basic shape of a~closed loop is a~circle. The idea behind these algorithms is exploration of the area of interest through riding circles. The data from circles can be evaluated in order to get estimation of direction in which the source should be searched for, or even estimation of the source’s location. It is worthwhile to develop algorithms incrementally from easy cases to the more exacting ones. Elements common to all proposed circular algorithms are described first.

\subsection{Generation of waypoints}

A standard form for the equation of circle is used to describe all circles in this thesis.

\begin{equation}
(x-x_c)^2 + (y-y_c)^2 = r^2
\end{equation}

Where \pnt{x_c,y_c} are coordinates of circle center and $r$ is its radius. Parameters of a~circle can be described by a~vector \vect{x_c,y_c,r}. The waypoints are generated in counterclockwise direction, therefore the main detector is aimed outside of a~circle and the secondary detector inside. It is convenient for a~robot to approach the circle along its tangent, because when it reaches the circle, no additional maneuvers are needed. First it is necessary to find points of tangency for a~line passing through the current position of robot $R$\pnt{x_R,y_R}. The points are given as intersections of a~circle and the polar line to point $R$. They can be obtained by solving the following system of equations:

\begin{subequations}
\begin{align}
x^2 + y^2&= r^2\\
(x_R-x_c)x + (y_R-y_c)y - r^2&= 0\label{eq.polar}
\end{align}
\end{subequations}

Where Eq.~\ref{eq.polar} represent equation of a~polar line. The solution leads to quadratic equation.

\begin{equation}
\left[1+\frac{(x_R-x_c)^2}{(y_R-y_c)^2}\right]x^2 - \left[\frac{2r^2 (x_R-x_c)}{(y_R-y_c)^2}\right]x + \left[\frac{r^4}{(y_R-y_c)^2}-r^2\right] = 0
\end{equation}

The $y$ coordinates are defined by:

\begin{equation}
y = \frac{r^2-(x_R-x_c)x}{(y_R-y_c)}
\end{equation}

%The coordinates are then translated back to original system by adding coordinates of circle center. 
Afterwards it is needed to select point of tangency which is located in the right half-plane. It has to satisfy following inequality:

\begin{equation}
(y_R-y_c)x + (x_c-x_R)y - (y_R-y_c)x_R - (x_c-x_R)y_R < 0
\end{equation}

The generation of waypoints between current position and a~point of tangency is described by Eq.~\ref{eq.pntcount} and Eq.~\ref{eq.pntcoords}. It is reasonable to omit the first half of waypoints in order to provide the robot with a~maneuvering space because it is not probable that it has a~proper yaw. When the current position of robot is inside the circle, it is not possible to approach the circle along tangent. The most convenient way is then using an arc. A minimal difference between the robot’s yaw at the end of arc and at the beginning of circle should be achieved. The robot lies inside circle when inequality is satisfied:

\begin{equation}
(x-x_c)^2 + (y-y_c)^2 < r^2
\end{equation}

The arc lies on some circle which needs to be defined first. A ray with current position as an initial point and current yaw as a~direction is found. Then intersection of the ray and circle is calculated. Half of a~distance between current position and the intersection is going to be the radius of the arc circle. Coordinates of the current point are then translated by the radius in a~direction perpendicular to current yaw. The new point can be the center of the arc circle if intersections between the original circle and potential arc circle exists. If the intersections do not exists, the radius is increased and the process is repeated until suitable arc center is found. Intersections of circle \vect{x_1,y_1,r_1} and circle \vect{x_2,y_2,r_2} exist if the following quadratic equation has solution:

\begin{equation}
\left[1+\frac{(x_2-x_1)^2}{(y_2-y_1)^2}\right]x^2 - \left[\frac{(x_2-x_1)c}{(y_2-y_1)^2}\right]x + \left[\frac{c^2}{4(y_2-y_1)^2}-r_1^2\right] = 0
\end{equation}
where

\begin{equation}
c = r_1^2 - r_2^2 + (x_2-x_1)^2 + (y_2-y_1)^2
\end{equation}

The $y$ coordinates are then given by:

\begin{equation}
y = \frac{c-2(x_2-x_1)x}{2(y_2-y_1)}
\end{equation}

When the arc circle is known so are the endpoints of the arc (current position \pnt{x_1,y_1} and intersection of the circles \pnt{x_2,y_2}), waypoints along the arc may be generated. Angular coordinate of a~point at circle \vect{x_c,y_c,r} is given by function:

\begin{equation}\label{eq.angle}
\phi(x,y)=
  \begin{cases}
    \arccos\frac{x-x_c}{r},& \text{if } y\leq y_c\\
    \arccos\frac{x_c-x}{r},              & \text{otherwise}
  \end{cases}
\end{equation}

Count of the waypoints between endpoints is:

\begin{equation}
n = \floor*{\frac{|\phi(x_2,y_2)-\phi(x_1,y_1)|r}{s}} + 1
\end{equation}
where $s$ is waypoint spacing. Coordinates of $i$-th point where $0 \leq i \leq n$ are given by these equations:

\begin{subequations}
\begin{align}
\phi_i&= \phi(x_1,y_1) + i\frac{|\phi(x_2,y_2)-\phi(x_1,y_1)|}{n}\\
x_i&= x_c +r\cos\phi_i\\
y_i&= y_c +r\sin\phi_i
\end{align}
\end{subequations}

Finally, waypoints at the circle are generated in the same manner as in case of the arc, the only difference is that start and end points are identical. The start point is either a~point of tangency in case the current position is outside circle, or an end point of arc otherwise. Obviously, the waypoints need to be transformed to WGS-84 in the end.

When the area of interest is explored from multiple circles, the preceding procedure is repeated for each one. The nearest one of the remaining circles is selected for processing, assuming the current position is equal to the end point of the previous circle. An example of generated sequence of waypoints (before they are transformed to WGS-84) for six circles and initial position \pnt{48,-24} and initial yaw 210° is shown in Fig.~\ref{fig.circwps}.

\begin{figure}[!h]
  \begin{center}
    \includegraphics[scale=0.8]{images/waypoints.pdf}
  \end{center}
  \caption{Generated sequence of waypoints for six circles}
  \label{fig.circwps}
\end{figure}

\subsection{Estimation of direction}\label{sec.estimation}

Assuming the robot maintains a~constant speed once it reaches a~circle, a~cyclic signal with equidistant data points is result of the measurement. According to theory, measured total count coming from a~source should be highest in intersection of the circle and the ray connecting the circle center with position of the source ({\it directional point}). Total count measured in neighboring points should be smaller because of their larger distance from the source. Also, the ratio of total count measured by main and by secondary detector should be extreme in the directional point. Either way, it is necessary to find dominant peaks in selected quantity – sum or ratio of total counts. Utilization of sum of total counts is assumed further in the text.

Once the peaks are identified, it is convenient to fit its neighborhood by appropriate function. There are several reasons for doing that. First, due to dead time, the point in right direction may not have the maximal total count. Also, it is probable that the actual maximum is somewhere between the samples. An interpolation provides subsample precision. For similar reasons as in Sec.~\ref{sec.interp} a~quadratic function is used.

Neighborhood for interpolation is defined by a~threshold which is relative to peak value. First endpoint is a~first point on the left from a~peak which has lower value than the threshold, analogically second endpoint is a~first point on the right from a~peak which has lower value than the threshold. When the endpoints cannot be found, no source is detected from the circle. Parameters of the interpolation function $ax^2+bx+c$ is computed by the least squares method \cite{parabola}.

\begin{equation}
\left \lbrack
	\begin{tabular}{c}
		$a$ \\
		$b$ \\
		$c$ 
	\end{tabular}
\right \rbrack
=
\left \lbrack
	\begin{tabular}{ccc}
		$\sum_{i=1}^n x_i^4$ & $\sum_{i=1}^n x_i^3$ & $\sum_{i=1}^n x_i^2$\\
		$\sum_{i=1}^n x_i^3$ & $\sum_{i=1}^n x_i^2$ & $\sum_{i=1}^n x_i$\\
		$\sum_{i=1}^n x_i^2$ & $\sum_{i=1}^n x_i$ & $n$
	\end{tabular}
\right \rbrack
^{-1}
\left \lbrack
	\begin{tabular}{c}
		$\sum_{i=1}^n y_ix_i^2$ \\
		$\sum_{i=1}^n y_ix_i$ \\
		$\sum_{i=1}^n y_i$ 
	\end{tabular}
\right \rbrack
\end{equation}
where index of point in measured sequence represents its $x$ value, and $n$ is count of points. Index of maximal value of the quadratic function $i_{max}$ is given as:

\begin{equation}
i_{max} = -\frac{b}{2a}
\end{equation}

Due to asymmetric directional characteristics of detectors and fact that measurements are took in motion, there is a~shift between the computed and actual maximum. It can be compensated by adding peak shift correction value $i_{corr}$ to $i_{max}$. A position of directional point \pnt{x_d,y_d} can be get by linear interpolation:

\begin{subequations}
\begin{align}
i_d&=i_{max} + i_{corr}\\
x_d&= x(\floor*{i_d})+(i_d-\floor*{i_d})[x(\ceil*{i_d})-x(\floor*{i_d})]\\
y_d&= y(\floor*{i_d})+(i_d-\floor*{i_d})[y(\ceil*{i_d})-y(\floor*{i_d})]
\end{align}
\end{subequations}

If the total count measured by the main detector in peak is higher than the corresponding value measured by a~secondary detector, the source is located outside the circle. Then its direction from the directional point can be expressed using Eq.~\ref{eq.angle}. Otherwise the source lies inside the circle and its direction is opposite.

\subsection{Navigation to source}

Once the initial point and direction are known, it is possible to navigate the robot towards the source. Trajectory is denoted as {\it directional line}, although it is not a~line because the direction is altered as new data are being measured.

\begin{figure}[!h]
  \begin{center}
    \includegraphics[scale=1.5]{images/ray.pdf}
  \end{center}
  \caption{Determining a~direction of point}
  \label{fig.ray}
\end{figure}

Basic geometric primitive which is used here is a~ray which is described by initial point $A$\pnt{x_a,y_a} and direction $\phi$. There is no analytic expression for a~ray, it may be described by an equation of a~line $p_1$ at which the ray lies, and auxiliary perpendicular line $p_2$ passing through the initial point. Both lines are shown in Fig.~\ref{fig.ray}. Their equations may be expressed as:

\begin{subequations}
\begin{align}
p_1:& \tan(\phi) x - y + y_d-\tan(\phi) x_d=0\\
p_2:&
  \begin{cases}
    -x-\tan(\phi) + \tan(\phi)y_d+x_d=0,& \text{if } -90^{\circ}<\phi<90^{\circ}\\
    x+\tan(\phi) - \tan(\phi)y_d - x_d=0,& \text{if } 90^{\circ}<\phi<270^{\circ}
  \end{cases}\\
\end{align}
\end{subequations}

Purpose of the auxiliary line is in determining if a~point lies in the same half-plane as the ray ($P_1$) or in the opposite one ($P_2$). If the following inequality is satisfied, both point and ray are in same half-plane.

\begin{equation}\label{eq.ray}
p_2(x_p,y_p) > 0
\end{equation}

The reason is that the point lies in a~direction of normal vector of the auxiliary line. Using this technique, it is possible to find intersection of a~ray and polygon. Initial directional line is composed of waypoints generated between the directional point \pnt{x_d,y_d} and intersection with polygon in corresponding direction $\phi$. It is thereby assured that no waypoint is generated outside polygon which must not be left by the robot. One extra waypoint is generated in the opposite direction in order to provide the robot with maneuvering space so it reaches the beginning of the directional line with correct yaw. Coordinates of this waypoint are defined by:

\begin{subequations}
\begin{align}
x&= x_d + d\cos\phi\\
y&= y_d + d\sin\phi
\end{align}
\end{subequations}
where $d$ is desired distance from the directional point. Since the first data point is measured at the directional line, a~relative direction of the source is evaluated with each following measurement. Given current readings from the main detector $M(t)$ and from secondary detector $S(t)$, as well as the previous readings $M(t-1)$, $S(t-1)$, it is possible to decide whether the source is located in front of the robot or whether it is on the right, resp. on the left. When the following condition is fulfilled, the source is detected on the right:

\begin{equation}
[M(t) \geq K\cdot S(t)] \land [M(t-1) \geq S(t-1)] \land \left[\frac{M(t)}{S(t)} \geq \frac{M(t-1)}{S(t-1)}\right]
\end{equation}
where $K$ represents insensibility which should suppress influence of noise. Condition for detecting the source on the left is analogical:

\begin{equation}
[S(t) \geq K\cdot M(t)] \land [S(t-1) \geq M(t-1)] \land \left[\frac{S(t)}{M(t)} \geq \frac{S(t-1)}{M(t-1)}\right]
\end{equation}
Both conditions are appended by a~secondary one.

\begin{equation}
[M(t)+S(t)] \geq [M(t+1)+S(t+1)]
\end{equation}
which assures that in case that the robot already passed the source, trajectory is not updated. The reason is that previous conditions could fail when the source is behind a~robot due to deformation of the directional characteristics. When both primary and secondary conditions are fulfilled, direction is updated by $+\Delta\phi$, resp. $-\Delta\phi$. Then a~new sequence of waypoints is generated from current position to intersection with polygon in the new direction.

There are two terminating conditions for this partial algorithm. The first one is fulfilled when the source has been provably passed by the robot. In that case it is possible to interpolate data points measured at direction line with quadratic function in a~similar way as in Sec.~\ref{sec.estimation}. Afterwards, the source is marked as {\it found}, and the final loop may be planned in a~manner described in Sec.~\ref{sec.strong}. It is possible to omit loops in configuration file.

The second terminating condition is fulfilled when the source could not be found within allowed distance. If the source is located outside the circle, navigation is terminated when final generated waypoint at an edge of the polygon has been reached. Otherwise, the maximal allowed distance is equal to a~diameter of the circle. Either way, the source is marked as {\it not found}.

\subsection{First circular algorithm}

The simplest situation is when only one radiation source is present in the area of the interest, and the area is small enough to be covered by single circle. This algorithm represents an alternative to strong source search algorithm presented in Sec.~\ref{sec.strong}. Since only one peak is expected in circle data points, it can be easily found as the maximal value in data set. After the direction is estimated, the source is navigated to using the described continuous update of trajectory. Schematic example of such measurement trajectory is shown in Fig.~\ref{fig.circ1traj}. Actual location of the source is marked by the red point, the black line represents initial estimation of direction.

\begin{figure}[!h]
  \begin{center}
    \includegraphics[scale=0.8]{images/trajectory_circ1.pdf}
  \end{center}
  \caption{Schematic of the first circular algorithm trajectory}
  \label{fig.circ1traj}
\end{figure}

\subsection{Second circular algorithm}

A slightly more complicated is a~case when multiple source are present in the area of interest, but all of them may be detected from a~single circle and are sufficiently separated by angle. Therefore, development of peak detector is needed. Real data are very noisy, thus the detector needs to be robust. {\it Simple peak} is defined as a~point which has value (in this case total count) greater than its two neighboring points. A large amount of simple peaks could be detected in real data, most of them would not match actual radiation source. What is needed to be searched for are {\it prominent peaks}.

Algorithm integrated in MATLAB’s \texttt{findpeaks} function is adapted \cite{findpeaks}. Exemplary cyclic signal can be seen in Fig.~\ref{fig.findpeaks} and the peak detection will be illustrated on it. The signal has three simple peaks (1, 2, 3) and three valleys (a, b, c). Peaks are evaluated using a~reference level that has to be defined first. In order to find it, the following steps are repeated for each peak:
\begin{enumerate} 
	\item Extend a~horizontal line from the peak to the left and right until the line crosses the signal (a~point with greater or equal value is crossed).
	\item Find the minimum of the signal in each of the two intervals defined in Step 1. This point is one of the valleys.
	\item The higher of two interval minima specifies the reference level.
\end{enumerate} 

\begin{figure}[!h]
  \begin{center}
    \includegraphics[scale=0.65]{images/findpeaks.pdf}
  \end{center}
  \caption{Illustration for finding peaks}
  \label{fig.findpeaks}
\end{figure}

Peaks shown in Fig.~\ref{fig.findpeaks} would have the following reference levels:

\begin{tabular}{ll}
1:&a\\
2:&c\\
3:&c
\end{tabular}

\vspace{8pt}

If the peak amplitude is greater or equal to the reference value multiplied by desired relative prominence, the peak is accepted as prominent. If the relative prominence matches the threshold for interpolation by quadratic function, it is assured the interpolation may be done.

After all peaks are evaluated, direction of multiple sources can be estimated. The estimations may be ordered for example by the matching peak amplitude. Higher amplitude corresponds to either stronger source or closer source. In first case, stronger sources are located first. In other case, it is convenient to detect closer sources first, because the robot has to return to the circle after a~source is found.

Localization of the sources is performed in sequence using the earlier described navigation to source algorithms for each one. After one source is marked as found or not found, the robot returns to the beginning of another directional line located on the circle. Algorithm ends when all potential sources are explored. Schematic example of such trajectory can be seen in Fig.~\ref{fig.circ2traj}.

\begin{figure}[!h]
  \begin{center}
    \includegraphics[scale=0.8]{images/trajectory_circ2.pdf}
  \end{center}
  \caption{Schematic of the second circular algorithm trajectory}
  \label{fig.circ2traj}
\end{figure}

\subsection{Third circular algorithm}

Third algorithm is concerned with a~situation when the area of interest cannot be explored by single circle, therefore multiple circles are employed. However, an important simplification is assumed – the sources do not interfere with each other. In other words, every source evokes prominent peak on only one circle.

First, the robot rides round all the circles. The data are then evaluated in a~familiar way – directions of sources are estimated and then each of the potential sources is tried to be located. Once the source is (or is not) located, it is convenient to proceed with a~source that has directional point with the lowest distance from the current position. An example of such trajectory is shown in Fig.~\ref{fig.circ3traj}.

\begin{figure}[!h]
  \begin{center}
    \includegraphics[scale=0.6]{images/trajectory_circ3.pdf}
  \end{center}
  \caption{Schematic of the third circular algorithm trajectory}
  \label{fig.circ3traj}
\end{figure}

\subsection{Fourth circular algorithm}

The last proposed algorithm covers the most general case – the area of interest is explored from multiple circles, while several sources are present and interfere with each other. That, on the other hand, allows development of algorithm which estimates the location of the source, not only its direction. 

\begin{figure}[!h]
  \begin{center}
    \includegraphics[scale=0.7]{images/trajectory_circ4.pdf}
  \end{center}
  \caption{Schematic of the fourth circular algorithm trajectory}
  \label{fig.circ4traj}
\end{figure}

The algorithm is described using a~situation illustrated in Fig.~\ref{fig.circ4traj}. It can be seen that three circles denoted as 1, 2 and 3 are present, as three radiation sources. Seven peaks are found on the circles in total. Each of them has corresponding ray marking the direction of source, these rays are illustrated by black lines. Estimation of the location of the sources may be divided into the following steps:
\begin{enumerate} 
	\item For each ray, intersections with other rays are found.
	\item Discard all intersections located outside the area of interest.
	\item If some ray corresponds to the source detected inside the circle, discard its intersections outside the circle.
	\item For each pair of rays, accept only one intersection that is nearest to one of theirs initial points and second nearest to the other one at most.
\end{enumerate} 


The area of interest is defined by ordered sequence of polygon vertices as it was stated earlier in Sec.~\ref{sec.mapping}. To examine whether a~point $A$ lies inside or outside polygon, an algorithm called ray casting may be utilized \cite{ray-cast}. First, a ray originating in the examined point is found. Then, all intersection of this ray and polygon are counted using Eq.~\ref{eq.intersections}, Eq.~\ref{eq.polygonint} and Eq.~\ref{eq.ray}. If the number of intersections is even, the point lies outside a~polygon. Otherwise, it is inside. All intersections satisfying this conditions are marked in Fig.~\ref{fig.circ4traj}, using code {\it cc}\#, where {\it cc} are labels of two circles where the intersecting rays are originated on, and \# is order in sequence. Result of the selection algorithm is shown in Tab.~\ref{tab.intersections}.

\begin{table}[ht]
\catcode`\-=12

\begin{center}
\caption{Overview of found intersections}
\label{tab.intersections}

\begin{tabular}{|l|p{11cm}|}

\hline
{\bf Intersection}&{\bf Accepted/discarded}\\
\hline
12a&Accepted.\\
\hline
12b&Accepted.\\
\hline
12c&Discarded because it lies outside circle 2.\\
\hline
13a&Discarder because intersections 13b and 23a are nearer to the initial point on circle 3.\\
\hline
13b&Accepted.\\
\hline
13c&Accepted.\\
\hline
23a&Accepted.\\
\hline
23b&Discarded because intersections 23a and 12b are nearer to the initial point on circle 2.\\
\hline

\end{tabular}
\end{center}
\end{table}

After an application of the algorithm, there are still five intersections left although only three sources are present in the area. The reason is that intersection 12b, 13b and 23a mark the same source but, due to inaccuracy in measurement, the matching rays do not intersect in a~single point. A possible solution is to iteratively replace intersections which have a~distance lower than some threshold $D_{min}$ by theirs average coordinates until all intersections are well separated.
\begin{enumerate} 
	\item Intersections 12b and 13b are united into point P$_1$.
	\item Intersection 23a and point P$_1$ are united into point P$_2$.
	\item All remaining intersections and points have mutual distance greater than $D_{min}$.
\end{enumerate} 

\vspace{8pt}

Final estimations of sources’ position are marked with red crosses. In order to maintain the figure well arranged, only one directional line with matching loop is displayed. After the source is found, the robot would proceed to another one and finally to the third one.

It is possible that some source is located somewhere nearby the edge of the area and is detectable from only one circle. In that case, direction of the source is still known and it can be found by means presented in the previous three algorithms. Flowchart describing whole algorithm is shown in Fig.~\ref{fig.circflowchart}.

\begin{figure}[!h]
  \begin{center}
    \includegraphics[scale=0.385]{images/flowchart_circ4.pdf}
  \end{center}
  \caption{Flowchart of the fourth circular algorithm}
  \label{fig.circflowchart}
\end{figure}