Optimal Observability Problem in Trajectory Planning

• OOP is the principled design of sensor trajectories that maximize information about unknown parameters using optimal control and Fisher information measures.
• The approach fuses dynamic trajectory planning with information theory by employing bang and singular arcs to optimize the determinant of the Fisher information matrix.
• Numerical methods confirm that principal symmetric solution branches outperform nonsymmetric paths, effectively reducing estimator uncertainty in bearing-only estimation scenarios.

The Optimal Observability Problem (OOP) refers to the principled design of sensor (observer) trajectories or input signals to maximize the information acquired about unknown parameters of a system, subject to dynamic and operational constraints. In the prototypical scenario studied in "Observer Path Planning for Maximum Information," the OOP is cast as an optimal control problem: an observer moving at constant speed aims to estimate the position of a stationary target using only continuous, noisy bearing measurements, and the goal is to plan the observer's trajectory to maximize the determinant of the Fisher information matrix (FIM) accumulated over a fixed observation time. This problem illustrates central themes of OOP in information-based estimation and optimal control, including the fusion of dynamic trajectory planning with information-theoretic criteria, the emergence of optimal "excitation" patterns in sensor motion, and the complex structure of optimal solutions.

1. Mathematical Formulation: Observer Dynamics and Information Objective

The observer moves in the plane at fixed speed $v>0$, with inertial position $(x_s(t),y_s(t))$ and heading angle $\theta(t)$. Its state evolution is given by

$$\dot x_s = v \cos\theta, \qquad \dot y_s = v \sin\theta.$$

The unknown, stationary target is at $(x_T, y_T)$. The observer measures the target's bearing with additive white Gaussian noise: $$\tan(\beta(t) + w(t)) = \frac{y_s(t) - y_T}{x_s(t) - x_T}, \qquad w(t) \sim N(0, \sigma^2).$$ The relative position $(x, y) = (x_s - x_T, y_s - y_T)$ evolves as

$$\dot x = v \cos\theta, \qquad \dot y = v \sin\theta,$$

with initial state $(x(0), y(0)) = (x_0, y_0)$. The information reward is the Fisher information matrix for $(x_T, y_T)$, accrued continuously from the bearing measurements: $$F(\theta) = \frac{1}{\sigma^2} \begin{pmatrix} \int_0^{t_f} \frac{y^2(\tau)}{(x^2(\tau) + y^2(\tau))^2} d\tau & -\int_0^{t_f} \frac{x(\tau)y(\tau)}{(x^2(\tau)+y^2(\tau))^2} d\tau \ -\int_0^{t_f} \frac{x(\tau)y(\tau)}{(x^2(\tau)+y^2(\tau))^2} d\tau & \int_0^{t_f} \frac{x^2(\tau)}{(x^2(\tau)+y^2(\tau))^2} d\tau \end{pmatrix}.$$ The scalar measure $\det F(\theta)$ serves as the objective, as it is inversely proportional to the area of the target's uncertainty ellipse at the estimator's output.

2. Optimal Control Problem Structure

The OOP is formulated as a Mayer-type optimal control problem:

• States: $(x, y, z_1, z_2, z_3)$, where
  • $z_1(t)$ accumulates $\int_0^t \frac{x^2}{(x^2+y^2)^2} d\tau$,
  • $z_2(t)$ accumulates $\int_0^t \frac{y^2}{(x^2+y^2)^2} d\tau$,
  • $z_3(t)$ accumulates $\int_0^t \frac{xy}{(x^2+y^2)^2} d\tau$.
• Control: heading angle $\theta(t)$; in practical settings, control is the curvature $u(t)$ via $\dot\theta = u/v$, bounded by $|u(t)| \leq v a$.
• Boundary Conditions: initial $(x_0, y_0)$, $z_i(0) = 0$, fixed terminal time $t_f$.
• Objective:

$$J = z_3^2(t_f) - z_1(t_f) z_2(t_f) = -\det F(\theta) \cdot \sigma^4,$$

to be minimized.

3. Application of the Pontryagin Maximum Principle

The Hamiltonian for the control-affine system (with bounded curvature and heading as a control variable) is

$$H = \lambda_x v\cos\theta + \lambda_y v\sin\theta + \lambda_\theta \frac{u}{v} + \lambda_{z_1} \frac{x^2}{(x^2 + y^2)^2} + \lambda_{z_2} \frac{y^2}{(x^2 + y^2)^2} + \lambda_{z_3} \frac{xy}{(x^2 + y^2)^2},$$

with state and costate/Cauchy conditions fully specified. The necessary conditions of optimality are:

• Adjoint equations: $\dot \lambda_x = -\frac{\partial H}{\partial x}$, $\dot \lambda_y = -\frac{\partial H}{\partial y}$, etc.
• Transversality: terminal costate conditions link $\lambda_x(t_f)$, $\lambda_y(t_f)$, etc., to the derivatives of the endpoint Mayer cost.
• Optimal control law: for bounded curvature,

$$u^*(t) = \begin{cases} +va, & \lambda_\theta(t) < 0 \ -va, & \lambda_\theta(t) > 0 \ \text{singular arc}, & \lambda_\theta(t) \equiv 0 \end{cases}$$

Singular arcs correspond to heading evolution $$\theta_{\rm sing}(t) = \arctan(\lambda_y/\lambda_x)$$.

4. Analytical Structure of Optimal Trajectories

Optimal observer paths generically concatenate:

• Bang arcs: Maximum curvature; motion on constant-radius circles ("tight turns").
• Singular arcs: Curvature within bounds, heading tracks the costate structure according to $\theta_{\rm sing}$. Typical optimal trajectories are of "bang–singular" or "bang–singular–bang" concatenated form, with switches governed by the zero-crossings of $\lambda_\theta(t)$. For the canonical scenario with a stationary target at the origin, short observation times favor a simple "bang–singular" profile, while longer times can exhibit symmetric "bang–singular–bang" solutions.

5. Numerical Solution and Locally Optimal Branches

The OOP is solved numerically by direct discretization (trapezoidal or forward Euler, with up to 2,000 time points) of the state/costate ODEs and Mayer cost, posed as a nonlinear program (NLP) that is solved by interior-point methods (AMPL + Ipopt). Validity of the solution is checked by comparison of the signs of the switching function with computed $\dot\theta(t)$.

Empirical findings include:

• Principal solution branch: symmetric with respect to the initial geometry, usually attains the globally maximal information measure.
• Secondary branches: nonsymmetric, corresponding to local optima that satisfy first-order conditions but achieve lower $\det F$.
• The principal solution always achieves higher information (smaller estimator uncertainty) than nonsymmetric branches for the same duration and geometric constraints.

6. Generalization and Insights for OOP

Key general principles illuminated by this canonical OOP:

• Information functional maximization (e.g., $\det F$) leads to trajectories that maximize estimator performance by fostering persistent excitation, thus reducing estimator covariance.
• Trajectory geometry: bearing-only estimation is maximally informative when the observer's path creates significant variation in bearing angle, producing a well-conditioned FIM. This is realized via sequences of high-curvature (bang) and information-maintaining (singular) motions.
• This paradigm readily generalizes:
  • Moving targets: FIM structure must then account for additional parameters and nonautonomous effects.
  • Multiple/heterogeneous observers: Coordination poses a multi-agent OCP.
  • Alternative information metrics: Trace, minimum eigenvalue, and multi-objective variants can be adopted.
  • State constraints: Obstacles or minimum-range requirements are introduced as path constraints.
  • Sensor models: Inclusion of range or range-rate measurements leads to more complex, possibly higher-rank, FIMs.

7. Conclusion and Implications

The OOP as demonstrated in bearing-only sensor trajectory planning establishes a rigorous optimal control approach to maximizing estimation efficiency subject to vehicle and sensing constraints. The theoretical framework, via characterization of necessary conditions and solution structure, directly informs the design of autonomous observer trajectories in robotics, tracking, and surveillance. The analysis shows that optimal paths are generically composed of bang (maximal excitation) and singular (information-preserving) arcs, with optimal switching as dictated by the Pontryagin maximum principle. The numerical optimization pipeline is robust and reveals the existence of multiple locally optimal solutions, while consistently identifying the symmetric (“principal”) branch as optimal according to the determinant criterion. These insights generalize beyond bearing-only estimation and serve as a foundation for more complex OOPs in multi-agent, constrained, or nonlinear estimation problems (Kaya, 2021).