# About Fisher Information Matrix and Self-Optimizing Control in Bearing Only Localization

Alexandre Genadot, Enzo Iglésis, Luc de Montella

<figure><img src="image_1.png" /></figure>

## To cite this version:

Alexandre Genadot, Enzo Iglésis, Luc de Montella. About Fisher Information Matrix and Self-Optimizing Control in Bearing Only Localization. 2025. hal-04894826

HAL Id: hal-04894826

https://inria.hal.science/hal-04894826v1

Preprint submitted on 17 Jan 2025

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_2.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# About Fisher Information Matrix and Self-Optimizing Control in Bearing Only Localization

Alexandre Génadot¹, Enzo Iglésis², and Luc de Montella*²,³

¹Univ. Bordeaux, CNRS, INRIA, Bordeaux INP, IMB, UMR 5251, F-33400 Talence, France.

²Inria centre at the University of Bordeaux, Talence, 33405, France

³Naval Group, Bouguenais, 44340, France.

January 17, 2025

## Abstract

In this article, we revisit the results of *Optimal observer motion for localization with bearing measurements* from Hammel, Liu, Hilliard, and Gong [1] concerning the lower bound associated with the determinant of the Fisher information matrix for bearing only localization. We specify this bound and use the associated optimality curves to construct an adaptive control for locating the position of a target using only noisy bearing measurements. For this self-optimizing control, the target position estimate is updated using a contrast minimum estimator whose strong consistency is proved. Numerical simulations allow us to discuss the effectiveness of the proposed method.

## 1 Introduction

We consider the classical problem of bearings-only tracking (BOT) where an observer wants to infer the position of some target over some time window $[0, T]$, from scalar measurements of the form

$$z(t) = h(\mathbf{x}(t)) + \eta(t) \tag{1}$$

where, for $t \in [0, T]$, the state vector $\mathbf{x}(t) \in \mathbb{R}^2$ given by

$$\mathbf{x} = [x \ y]^T \tag{2}$$

is the target position w.r.t. the observer, $\eta$ is a zero mean white Gaussian noise with variance $\sigma^2$ and the measurement function $h(\cdot)$ is given by

$$h(\mathbf{x}) = \begin{cases} \tan^{-1}\left(\frac{x}{y}\right) & \text{if } y \neq 0, \\ \frac{\pi}{2} & \text{if } y = 0 \text{ and } x > 0, \\ -\frac{\pi}{2} & \text{otherwise.} \end{cases} \tag{3}$$

This latter quantity corresponds to the bearing direction. In Figure 1, we present an illustration of the above situation where the target follows a linear motion with constant velocity. This problem has been of interest for many years, see for example [2, chap. 6] and [3] as well as the references therein.

In BOT problems, the received information often necessitates performing a maneuver to render the problem observable. While certain methods have been developed to incorporate additional information commonly available in practice (see [4]), thus reducing the need for a maneuver and enhancing observability, selecting an appropriate maneuver can eliminate the need for such supplementary information, such as frequencies, transmission loss, and others.

<footnote>*Luc de Montella would like to thank Naval Group for their financial support through the CIFRE PhD program.</footnote>
<figure><img src="image_3.png" /><figcaption>Figure 1: Bearing measurement ($\beta$) of a target ($\mathbf{x}^t$) in linear motion at constant velocity w.r.t. an ownship in linear motion at constant velocity.</figcaption></figure>

A common and efficient way to quantify the amount of information on $\mathbf{x}$ carried out by $z$ is the Fisher information matrix (FIM) denoted $\mathcal{I}(\mathbf{x})$, see for example [5, 6]. The determinant of this matrix is related to the area of an error ellipse for the state vector position. This determinant is equal to

$$|\mathcal{I}(\mathbf{x})| = \frac{1}{2\sigma^4} \int_{[0,T]^2} \frac{\sin(\beta(t) - \beta(s))^2}{r^2(t)r^2(s)} dt \, ds, \qquad (4)$$

where for $t \in [0, T]$, we used the polar coordinates

$$\mathbf{x}(t) = [r(t) \cos(\beta(t)) \quad r(t) \sin(\beta(t))]^\top. \qquad (5)$$

Then, if the observer pursues a trajectory that maximizes (4), maximum observability will be realized and the error ellipse will be minimized. However, it is often a derivation of the lower bound of the determinant of the FIM that is used, see [7, 5], instead of the exact formulae of the determinant given by (4), in the hope of being able to propose efficient and interpretable optimisation methods.

In the BOT setting, the authors of [1] claim, certainly by analogy with the result of [7], that

$$|\mathcal{I}(\mathbf{x})| \geq \frac{1}{\sigma^4} \left( \int_0^T \frac{\dot{\beta}(t)}{r^2(t)} dt \right)^2. \qquad (6)$$

This lower bound can be used to propose optimal trajectory with explicit analytical expression, see [1, Section 5]. However, a multiplicative term is missing in this expression as can be viewed from the simulation we proceed to present. In order to characterize the differences between the exact formulae given by (4) and its derivative lower bound given by the right-hand side of (6), we implement a scenario with a static target at the origin of the frame and an ownship at 5 km that moves in linear motion at constant velocity of 5 m s⁻¹ starting with $\beta = 45^\circ$. A total of 360 runs are performed with the ownship courses that vary of 1° between each simulation that last 300 s. This scenario is illustrated in Figure 2.

Thanks to these simulations, a numerical comparison of the value of the determinant of each run given by the right-hand side of (6) is then compared to the one given by (4). Figure 3 displayed the two resulting curves. We see that these two curves are qualitatively very closed, but the right and left scales show that they differ quantitatively from a factor $\approx 7380$.
<figure><img src="image_4.png" /><figcaption>Figure 2: Scenario of a moving ownership (blue) that measures the bearing of a static target (red), with 36 of the 360 headings.</figcaption></figure>

<figure><img src="image_5.png" /><figcaption>Figure 3: Determinant of the FIM (blue) from the scenario illustrated in Figure 2 for each course of the ownership, compared to its lowerbound approximation (red) given by the right-hand side of (6).</figcaption></figure>

In Section 2 we will show that an appropriate approximation of the lower bound is in fact

$$ \frac{T^2}{12\sigma^4} \left( \int_0^T \frac{\dot{\beta}(t)}{r^2(t)} dt \right)^2, \qquad (7) $$

giving the missing factor. In the presented scenario, this factor is $300^2/12 = 7500$ which is coherent with the simulation.

We would like to point out that from our point of view, the presence of this multiplicative factor, in a lot of cases, does not change the conclusions in [1]. Indeed, if we use the lower bound (6) to choose between two possible trajectories, the multiplicative factor is not important to compare the relative
goodness of these trajectories.

In [1], optimal trajectories are obtained when the target position is assumed to be known. In Section 3, we propose to place ourselves in the more realistic framework where this position $\mu$ of the target is assumed to be unknown. Our aim is then to estimate this position. For this purpose, we will construct a minimum contrast estimator of $\mu$, see [8, Chapter 5] and [9, 10] for recent works on this subject. Under quite realistic assumptions, this estimator will be shown to be strongly consistent, that is to converge almost-surely towards $\mu$, as long as the observer paths possess a prescribed minimum curvatures. Since the optimal curves provided by [1] have indeed a positive minimum curvature, we will then used the principle of estimation and control to propose observer paths able to infer the position of the target in an efficient way. This kind of control is said to be adaptative or self-optimizing. We address the perforance of the proposed method through the Cramer Rao lower bound (CRLB).

## 2 The Fisher Information Matrix and its lower bound

We begin to recall the definition of the FIM. We denote by $p(\cdot|\mathbf{x})$ the conditional probability distribution of the law of the observation given the state vector $\mathbf{x}$ which is Gaussian with mean $\mathbf{h}(\mathbf{x})$ and variance $\sigma^2$ such that

$$p(z|\mathbf{x}) = \frac{1}{\sigma\sqrt{2\pi}} \exp\left(-\frac{1}{2}\left(\frac{z-h(\mathbf{x})}{\sigma}\right)^2\right). \qquad (8)$$

In this case, the FIM at point $\mathbf{x}$ is well-defined, see [1], and is the $2 \times 2$ matrix $\mathcal{I}(\mathbf{x})$ given by:

$$\mathcal{I}(\mathbf{x}) = -\int_{\mathbb{R}} \frac{\partial^2 \log(p(z|\mathbf{x}))}{\partial \mathbf{x}^2} p(z|\mathbf{x}) \, dz. \qquad (9)$$

More precisely, the associated FIM of the state vector $\mathbf{x}$ is given by:

$$\mathcal{I}(\mathbf{x}) = \begin{bmatrix} \mathcal{I}(\mathbf{x})_{1,1} & \mathcal{I}(\mathbf{x})_{1,2} \\ \mathcal{I}(\mathbf{x})_{2,1} & \mathcal{I}(\mathbf{x})_{2,2} \end{bmatrix}, \qquad (10)$$

where FIM entry $\mathcal{I}(\mathbf{x})_{1,1}$, $\mathcal{I}(\mathbf{x})_{1,2}$, $\mathcal{I}(\mathbf{x})_{2,1}$ and $\mathcal{I}(\mathbf{x})_{2,2}$ are given by:

$$\mathcal{I}(\mathbf{x})_{1,1} = \int_{\mathbb{R}} \left( \frac{\partial \log(p(z|\mathbf{x}))}{\partial x} \right)^2 p(z|\mathbf{x}) \, dz, \qquad (11)$$

$$\mathcal{I}(\mathbf{x})_{1,2} = \int_{\mathbb{R}} \frac{\partial \log(p(z|\mathbf{x}))}{\partial x} \frac{\partial \log(p(z|\mathbf{x}))}{\partial y} p(z|\mathbf{x}) \, dz, \qquad (12)$$

$$\mathcal{I}(\mathbf{x})_{2,1} = \mathcal{I}(\mathbf{x})_{1,2} \qquad (13)$$

$$\mathcal{I}(\mathbf{x})_{2,2} = \int_{\mathbb{R}} \left( \frac{\partial \log(p(z|\mathbf{x}))}{\partial y} \right)^2 p(z|\mathbf{x}) \, dz. \qquad (14)$$

Some Gaussian and involved Freshman calculus leads to the following proposition, see [1, Equation (4)].

**Proposition 1.** The FIM at $\mathbf{x}$ for the BOT problem is given by:

$$\mathcal{I}(\mathbf{x}) = \frac{1}{\sigma^2} \begin{bmatrix} \frac{\sin(\beta)^2}{r^2} & -\frac{1}{2} \frac{\sin(2\beta)}{r^2} \\ -\frac{1}{2} \frac{\sin(2\beta)}{r^2} & \frac{\cos(\beta)^2}{r^2} \end{bmatrix}, \qquad (15)$$

where $[\beta \ r]^\top$ are the polar coordinates of $\mathbf{x}$.
Assuming that the measurements are taken continuously, the total information gathered during the time window $[0, T]$ is:

$$ \mathcal{I}(\mathbf{x}_{[0,T]}) = \frac{1}{\sigma^2} \left[ \begin{array}{cc} \displaystyle \int_0^T \frac{\sin(\beta(t))^2}{r^2(t)} dt & -\frac{1}{2} \int_0^T \frac{\sin(2\beta(t))}{r^2(t)} dt \\ -\frac{1}{2} \int_0^T \frac{\sin(2\beta(t))}{r^2(t)} dt & \displaystyle \int_0^T \frac{\cos(\beta(t))^2}{r(t)^2} dt \end{array} \right]. \quad (16) $$

**Proposition 2.** The determinant of (16) is given by

$$ |\mathcal{I}(\mathbf{x}_{[0,T]})| = \frac{1}{2\sigma^4} \int_{[0,T]^2} \frac{\sin(\beta(t) - \beta(s))^2}{r(t)^2 r(s)^2} dt \, ds. \quad (17) $$

We have calculated this determinant when the observer's motion is uniformly rectilinear, allowing us to have a proper lower bound for the FIM.

**Proposition 3.** Assume that the motion of the observer is linear with constant vector speed V and constant course given by some angle $\theta$. We have,

$$ \begin{aligned} |\mathcal{I}(\mathbf{x}_{[0,T]})| & - \frac{T^2}{12\sigma^4} \left( \int_0^T \frac{\dot{\beta}(t)}{r^2(t)} dt \right)^2 \\ & = \frac{1}{4\sigma^4 a^2 V^2} \left( \left( \tan^{-1}(\alpha + \beta) - \tan^{-1}(\alpha) \right)^2 - \frac{\beta^2}{(1+(\alpha+\beta)^2)(1+\alpha^2)} \right. \\ & \qquad \left. - \frac{1}{12}\beta^2 \left( \tan^{-1}(\alpha + \beta) - \tan^{-1}(\alpha) - \frac{\beta(\alpha(\alpha+\beta)-1)}{(1+(\alpha+\beta)^2)(1+\alpha^2)} \right)^2 \right) \end{aligned} $$

where,

$$ \alpha = \frac{x_0 \cos \theta + y_0 \sin \theta}{x_0 \sin \theta - y_0 \cos \theta}, \\ \beta = \frac{VT}{x_0 \sin \theta - y_0 \cos \theta} $$

*Proof.* Let $a = x_0 \sin(\theta) - y_0 \cos(\theta)$. We have:

$$ |\mathcal{I}(\mathbf{x}_{[0,T]})| = \frac{a^2 V^2}{\sigma^4} \left( \int_0^T \frac{t^2}{r(t)^4} dt \int_0^T \frac{1}{r(t)^4} dt - \left( \int_0^T \frac{t}{r(t)^4} dt \right) \right). \quad (18) $$

By setting $u = \frac{VT+b}{a}$, with $b = x_0 \cos(\theta) + y_0 \sin(\theta)$ and $du = \frac{V}{a} dt$, then it gives:

$$ \mathcal{I}(\mathbf{x}) = \frac{1}{\sigma^4 a^2 V^2} \left( \int_{\frac{b}{a}}^{\frac{b+VT}{a}} \frac{u^2}{(1+u^2)^2} du \int_{\frac{b}{a}}^{\frac{b+VT}{a}} \frac{1}{(1+u^2)^2} du - \left( \int_{\frac{b}{a}}^{\frac{b+VT}{a}} \frac{u}{(1+u^2)^2} du \right)^2 \right) \quad (19) $$

Each integral can be computed and we obtain

$$ \mathcal{I}(\mathbf{x}) = \frac{1}{4\sigma^4 a^2 V^2} \left( \left( \tan^{-1} \left( \frac{VT}{a} + \frac{b}{a} \right) - \tan^{-1} \left( \frac{b}{a} \right) \right)^2 - \frac{\left( \frac{VT}{a} \right)^2}{\left( 1 + \left( \frac{VT}{a} + \frac{b}{a} \right)^2 \right) \left( 1 + \left( \frac{b}{a} \right)^2 \right)} \right) \quad (20) $$
In the same manner,

$$ \frac{T^2}{12\sigma^4} \left( \int_0^T \frac{\dot{\beta}(t)}{r^2(t)} dt \right)^2 = \frac{1}{4\sigma^4 a^2 V^2} \frac{1}{12} \left( \frac{VT}{a} \right)^2 \left( \tan^{-1} \left( \frac{VT}{a} + \frac{b}{a} \right) - \tan^{-1} \left( \frac{b}{a} \right) - \frac{\frac{VT}{a} \left( \frac{b}{a} \frac{VT+b}{a} - 1 \right)}{\left( 1 + \left( \frac{VT}{a} + \frac{b}{a} \right)^2 \right) \left( 1 + \left( \frac{b}{a} \right)^2 \right)} \right)^2 \quad (21) $$

The result follows.

<figure><img src="image_6.png" /></figure>

It can be shown that these two expressions — (20) and (21) — agree up to order 5 when developing it around $\alpha$ thanks to a Taylor-Lagrange polynomial approximation. More precisely, there is some $\zeta \in (\alpha, \alpha + \beta)$ such that for any time window $[0, T]$,

$$ \mathcal{I}(\mathbf{x}) - \frac{T^2}{12\sigma^4} \left( \int_0^T \frac{\dot{\beta}(t)}{r^2(t)} dt \right)^2 = - \frac{2\beta^6(5\alpha^2 + 1)}{45(1 + \alpha^2)^6} + \frac{2\zeta^7(24\alpha^3 + \alpha)}{45(1 + \alpha^2)^7}. $$

For example, for the scenario developed in the introduction, Theorem 3 implies that

$$ \sigma^4 \left| \mathcal{I}(\mathbf{x}) - \frac{T^2}{12\sigma^4} \left( \int_0^T \frac{\dot{\beta}(t)}{r^2(t)} dt \right)^2 \right| \le 5.3 \times 10^{-14}. $$

Without the assumption of uniform rectilinear motion, some asymptotic results can be obtained such as the one presented in the next proposition.

**Proposition 4.** *At the neighbourhood of $T = 0$, we have,*

$$ |\mathcal{I}(\mathbf{x}_{[0,T]})| - \frac{T^2}{12\sigma^4} \left( \int_0^T \frac{\dot{\beta}(t)}{r^2(t)} dt \right)^2 \sim - \frac{T^3}{12\sigma^4} \frac{\dot{\beta}(T)}{r^2(T)}. $$

*Proof.* This is a direct consequence of the series expansion of the left hand side around $T = 0$. □

<figure><img src="image_7.png" /></figure>

## 3 Localisation Curves through estimation and control

In accordance with the results of Theorem 3, optimal observer paths are defined as those paths that maximize the quantity

$$ \int_0^T \frac{\dot{\beta}(t)}{r^2(t)} dt. $$

The analytical expression of these paths is obtained in [1, Section 4] when the target position is known. The optimal control is shown to be, for $t \in [0, T]$,

$$ \theta(t) = 3\beta(T) - 2\beta(t) + \cos^{-1}(0) $$

yielding the optimal paths

$$ r(t) = r(0) \left( \frac{\cos(3(\beta(T) - \beta(0)))}{\cos(3(\beta(T) - \beta(t)))} \right)^3. \quad (22) $$

Of course, in general, the position $\mu$ of the target is unknown. In Section 3.1, we propose to construct an estimator of $\mu$ and show its strong consistency, that is its almost-sure convergence towards $\mu$. We then use this estimator and the optimal path defined through (22) to propose in Section 3.2 efficient sub-optimal observer trajectories for the localization of the target.
## 3.1 Construction of a minimum contrast estimator of the target position

We assume that the target position $\mu \in \mathbf{M}$ is unknown but belongs to some known compact subset $\mathbf{M}$ of $\mathbf{R}^2$. We are going to construct a sequence of estimators $(\hat{\mu}_n)$ of $\mu$, converging towards $\mu$ almost-surely as long as the observer path has some minimal curvature. This condition of minimal curvature is necessary for the parameter $\mu$ to be identifiable and is satisfied by the optimal curves of the form (22) as it will be shown.

In order to properly define the sequence of estimators $(\hat{\mu}_n)$, let us recall the basic definition of the considered model. The target position with respect to the observer is denoted by $\mathbf{x} = \mathbf{x}^o - \mu$. We assume that the observer paths remain in a closed ball $\mathbf{B}$ that does not contain $\mu$, centred in $\mathbf{x}^o(0)$ with radius $3\tau CN_T$ where $\tau$ and $C$ are two positive reals and $N_T$ is the number of times that we will update the observer paths effectively. The constant $C$ is simply an upper bound for the observer speed. We assume that the sampling of the bearing is performed at frequency $\tau$ so that $3\tau$ denotes the time spent between two successive updates of the observer trajectory at time $t_k$ and $t_{k+1} = t_k + 3\tau$, with $t_0 = 0$. This is during these $3\tau$-time periods that we ask the observer's trajectory to have a strictly positive minimum curvature in order to ensure observability. To be able to define these specific trajectories, we need to enlarge our state variable by considering three successive observer positions, we define

$$ \mathbf{X}_k^o = (\mathbf{x}^o(3t_k - 2), \mathbf{x}^o(3t_k - 1), \mathbf{x}^o(3t_k)) \qquad (23) $$

for $t \ge 1$ with initial condition $\mathbf{X}_0^o = (\mathbf{x}^o(0), \mathbf{x}^o(0), \mathbf{x}^o(0))$. At time $t_{k+1}$, the observer choose its path such that

$$ \mathbf{X}_{k+1}^o = (A_k(\tau), A_k(2\tau), A_k(3\tau)) \qquad (24) $$

with $A_k \in \mathbf{A}(\mathbf{X}_k^o)$ where the admissible action set from $\mathbf{X} \in \mathbf{B}^3$ is defined by

$$ \mathbf{A}_{\epsilon}(\mathbf{X}) = \{f : [0, 3\tau] \to \mathbf{B} : \|f\|_{\text{Lip}} \le C, \epsilon \le \alpha(f(\tau), f(2\tau), f(3\tau)) \le \pi - \epsilon, f(0) = \mathbf{X}_3\} \qquad (25) $$

with $\epsilon > 0$. In the definition of this space, the function $\alpha$ gives the non oriented angle defined by its three entries: flat angles are prohibited so that the observer can not move in a straight line. Thus, the observer's trajectory possesses a minimal curvature parametrized by $\epsilon$. According to this dynamic, the state space for the augmented observer variable is then

$$ \mathcal{X}_0 = \{\mathbf{X}_0\} , \quad \mathcal{X}_{k+1} = \bigcup_{\mathbf{X} \in \mathcal{X}_k} \{(a(\tau), a(2\tau), a(3\tau)) : a \in \mathbf{A}_{\epsilon}(\mathbf{X})\} , \quad \mathcal{X} = \bigcup_{k=0}^{N_T} \mathcal{X}_k. \qquad (26) $$

Accordingly, we define the augmented variable for the observations by

$$ \mathbf{Z}_k = (\mathbf{z}(3t_k - 2), \mathbf{z}(3t_k - 1), \mathbf{z}(3t_k)) \qquad (27) $$

for $k \ge 1$ with initial condition $\mathbf{Z}_0 = (z_0, z_0, z_0)$. The admissible space of state-action pairs is then

$$ \mathbf{K} = \{(\mathbf{Z}, \mathbf{X}, a) : \mathbf{Z} \in \mathbf{R}^3, \mathbf{X} \in \mathcal{X}, a \in \mathbf{A}_{\epsilon}(\mathbf{X})\}. \qquad (28) $$

This space is composed of vectors of three successive observed angles and positions together with the associated admissible actions.

We are now able to give the definition of a contrast function on which the construction of our estimator will be based.

**Definition 1.** Let $\kappa$ be the function on $\mathbf{M} \times \mathbf{K} \times \mathbf{R}^3 \times \mathcal{X}$ such that

$$ \kappa(\mu, \mathbf{Z}, \mathbf{X}, a, \mathbf{Z}', \mathbf{X}') = - \sum_{i=1}^{3} \cos(h(a(i\tau) - \mu) - \mathbf{Z}_i). $$

Since this function only depends on $\mu$, $a$ and $\mathbf{Z}'$, we will simply write $\kappa(\mu, a, \mathbf{Z}')$ for its expression.
In this definition, the function $\kappa$ is continuous in $\mu$ thanks to the presence of the cosine function. The regularity of the contrast function is important both for the definition of our estimator and its consistency, the most important technical property to have being related to the uniform equi-continuity in $\mu$ of this family of functions.

We can now define the minimum contrast estimator associated to the contrast function $\kappa$.

**Definition 2.** We define, for $n \ge 1$ and any sequence of admissible state-action pairs $H_n \in \mathbf{K}^{n-1} \times \mathbf{R}^3 \times \mathcal{X}$

$$L_n(\mu, H_n) = \sum_{k=0}^{n-1} \kappa(\mu, A_k, \mathbf{Z}_k).$$

We then set

$$\hat{\mu}_n(H_n) = \underset{\mu \in \mathbf{M}}{\operatorname{argmin}} L_n(\mu, H_n). \qquad (29)$$

We will say that $(H_n)$ is an admissible history.

The estimator $(\hat{\mu}_n)$ is a variant of conditional least squares, see [8, Section 5.2] and [9], and belongs to the family of minimum contrast estimators. Note that it is well defined thanks to the compactness of the parameter space $\mathbf{M}$ and the continuity of the contrast function $\kappa$. Using the theory of adaptative Markov control processes developped in [8], we can obtained the strong consistency of the sequence of estimator $(\hat{\mu}_n)$ as stated in the following theorem.

**Theorem 1.** The sequence $(\hat{\mu}_n(H_n))$ is a strongly consistent sequence of estimators of $\mu$ for any choice of admissible history $(H_n)$.

*Proof.* Our aim is to apply the result [8, Theorem 3.6]. The transition kernel associated to our model, given the law of $(\mathbf{Z}_{k+1}, \mathbf{X}_{k+1}^o)$, knowing the current state $(\mathbf{Z}_k, \mathbf{X}_k^o)$ and the chosen admissible action $a_t$ is given by

$$Q_{\mu}(d\mathbf{Z}_{k+1}, d\mathbf{X}_{k+1}^o | \mathbf{Z}_k, \mathbf{X}_k^o, A_k) = \prod_{i=1}^{3} \frac{1}{\sigma\sqrt{2\pi}} e^{\frac{1}{2} \left( \frac{\mathbf{Z}_{k+1,i} - h(A_k(i\tau) - \mu)}{\sigma} \right)^2} d\mathbf{Z}_{k+1,i} \delta_{A_k(i\tau)}(d\mathbf{X}_{k+1,i}^o). \quad (30)$$

It is then not hard to see that $\kappa$ is indeed a contrast function in the sense of [8, Definition 2.1]. Indeed, $\kappa$ is bounded and for $\mu, \mu' \in \mathbf{M}$, $(\mathbf{Z}, \mathbf{X}, a) \in \mathbf{K}$ and $(\mathbf{Z}', \mathbf{X}') \in \mathbf{R}^3 \times \mathcal{X}$, we have

$$\int_{\mathbf{R}^3 \times \mathcal{X}} [\kappa(\mu', a, \mathbf{Z}') - \kappa(\mu, a, \mathbf{Z}')] Q_{\mu}(d\mathbf{Z}', d\mathbf{X}' | \mathbf{Z}, \mathbf{X}, a) = e^{-\frac{\sigma^2}{2}} \sum_{i=1}^{3} (1 - \cos(h(a(i\tau) - \mu) - h(a(i\tau) - \mu'))). \quad (31)$$

The above term is positive as lons as $\mu' \neq \mu$.

The compacity of the spaces $\mathbf{M}, \mathcal{X}$ and $\mathbf{A}_\epsilon(\mathbf{X})$ for $\mathbf{X} \in \mathcal{X}$, as well as the continuity of $\kappa$ in its arguments implie that the Assumptions 3.2 and 3.3 of [8, Section 5.3] are satisfied. Then the theorem [8, Theorem 3.6] holds true, that is, the sequence $(\hat{\mu}_n(H_n))$ is a strongly consistent sequence of estimators of $\mu$ for any choice of admissible history $(H_n)$. $\square$

## 3.2 Self-optimizing localization curves

Following the principle of estimation and control, we propose the following methodology to infer the position of the target from the analytical expression of the optimal paths (22) when the target position is known and the estimation of this target position by the minimum contrast estimator given by (29). The algorithm consists in the following steps:

1. At the sampling time $t = 0$, observe the bearing $\mathbf{z}(0)$, and therefore $\mathbf{Z}_0$, and choose an arbitrary value for $\hat{\mu}_0$ for the target position. It is possible to take into account the first observation to choose this value.

2. Choose as action $A_0$ the optimal curve given by (22) assuming that the target position is $\hat{\mu}_0$.
3. Compute the resulting observer position $\mathbf{X}_1^o$ and observe $\mathbf{Z}_1$.

4. From now on, we have enough information to begin to compute our estimation using the expression (29) of the minimum contrast estimator. Thus, compute $\hat{\mu}_1 = \hat{\mu}_1(\mathbf{Z}_0, \mathbf{X}_0^o, A_0, \mathbf{Z}_1, \mathbf{X}_1^o)$.

5. Choose as action $a_1$ the optimal curve given by (22) assuming that the target position is $\hat{\mu}_1$.

6. And so on and so forth.

The numerical implementation of this method is quite straightforward except for the effective calculation of the minimum contrast estimator which requires to minimize a continuous function of two variables on a compact set. There is numerous methods to do so. We propose to use an evolutionary algorithms as presented in [11].

To evaluate the accuracy and robustness of the method, we performed a simulation extending over 45 minutes. During this simulation, the target remains fixed at the position [4.5] in a Cartesian coordinate system where the coordinates are measured in kilometers. The observer's initial position is at [0 2]. The observer travels at a constant speed of 4 m s⁻¹, receives a new measurement every minute, and adjusts its maneuver every 3 min. Each measurement is affected by a white noise with a variance of 1.5 rad.

To compute the first trajectory, we generated an initial guess for the target's position by sampling from a Gaussian distribution centered 3 km beyond the true initial range, with a variance of 5 km, and used the first bearing measurement to determine direction.

We applied a Particle Swarm Optimization (PSO) algorithm with 1000 particles and 100 iterations to estimate the target's position. We employed the modified PSO algorithm from [12], with the inertia coefficient $w$ set to 0.07298, and cognitive and social acceleration coefficients, $c_1$ and $c_2$ set to 2.05. FIG. 4 presents an example of the resulting trajectory, which exhibits the begging of the classic spiral shape commonly regarded as optimal [13, 14].

<figure><img src="image_8.png" /><figcaption>Figure 4: Example of an observer's trajectory using the observability optimization algorithm</figcaption></figure>

FIG. 5 shows the evolution of the root-mean-square error (RMSE) for the method, calculated over 1000 runs. To give a more detailed view of the error distribution, the figure also includes a box plot, showing the variability and spread of the observed errors. In addition, for comparison purposes, the Cramér-Rao lower bounds (CRLB) for the Bearing-Only Tracking problem is shown. This bound serves as a benchmark, illustrating the theoretical minimum error achievable, which our method seeks to approach. The detailed derivation of the CRLB can be found in [2].

The first real estimate of the target's position is obtained after 3 min, the initial guess at time 0 being based on the initialization method detailed previously. At this point, the method's RMSE is
at 1.07 km. However, the high variance in the initial error suggests significant uncertainty in the first estimate, making it less reliable.

As additional measurements are collected and incorporated into the estimation process, the RMSE decreases rapidly. By the second estimation, the error drops significantly and stabilizes well below 1 km. This rapid reduction highlights the method's ability to quickly improve the target position estimate.

After the initial estimates, the RMSE approaches the CRLB, indicating that the method is performing near the theoretical optimal error. The spread in the errors, which is considerable in the early stages, narrows significantly as the method converges, demonstrating the consistency and accuracy of the approach over time.

Overall, the method demonstrates a rapid convergence and reliable accuracy, making it highly effective for real-time localization scenarios.

## References

[1] S. E. Hammel, P. T. Liu, E. J. Hilliard, and K. F. Gong, "Optimal observer motion for localization with bearing measurements," *Computers & Mathematics with Applications*, vol. 18, no. 1-3, pp. 171-180, 1989.

[2] B. Ristic, S. Arulampalam, and N. Gordon, *Beyond the Kalman filter: Particle filters for tracking applications*. Artech house, 2003.

[3] S. Nardone, A. G. Lindgren, and K. Gong, "Fundamental properties and performance of conventional bearings-only target motion analysis," *IEEE Transactions on automatic control*, vol. 29, no. 9, pp. 775-787, 1984.

[4] E. Iglésis and L. de Montella, "Target motion analysis using combined bearings and underwater sound pressure levels," Nov. 2024, working paper or preprint.

[5] J.-M. Passerieux and D. Van Cappel, "Optimal observer maneuver for bearings-only tracking," *IEEE Transactions on Aerospace and Electronic Systems*, vol. 34, no. 3, pp. 777-788, 1998.

[6] J.-P. Le Cadre and S. Laurent-Michel, "Optimizing the receiver maneuvers for bearings-only tracking," *Automatica*, vol. 35, no. 4, pp. 591-606, 1999.

[7] P. T. Liu, "An optimum approach in target tracking with bearing measurements," *Journal of Optimization theory and Applications*, vol. 56, pp. 205-214, 1988.

[8] O. Hernández-Lerma, *Adaptive Markov control processes*. Springer Science & Business Media, 2012, vol. 79.

[9] F. Dufour, A. Genadot, and O. L. V. Costa, "Minimum contrast estimators for piecewise deterministic markov processes," *Numerical Algebra, Control and Optimization*, pp. 0-0, 2024.

[10] O. L. V. Costa, F. Dufour, and A. Genadot, "Adaptive average control for piecewise deterministic markov processes," *Systems & Control Letters*, vol. 192, p. 105894, 2024.

[11] M. R. Bonyadi and Z. Michalewicz, "Particle swarm optimization for single objective continuous space problems: a review," *Evolutionary computation*, vol. 25, no. 1, pp. 1-54, 2017.

[12] Y. Shi and R. Eberhart, "A modified particle swarm optimizer," in *1998 IEEE International Conference on Evolutionary Computation Proceedings. IEEE World Congress on Computational Intelligence (Cat. No.98TH8360)*, 1998, pp. 69-73.

[13] P. T. Liu, "An optimum approach in target tracking with bearing measurements," *Journal of Optimization Theory and Applications*, vol. 56, no. 2, pp. 205-214, 1988.

[14] N. Adib and S. C. Douglas, "Assessing range accuracy for bearings-only geolocation using optimal logarithmic spiral sensor path trajectories," in *2015 IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP)*, 2015, pp. 2554-2558.
<figure><img src="image_9.png" /><figcaption>Figure 5: Evolution of the estimation error using the minimum contrast estimator and the observability optimization algorithm</figcaption></figure>