Observability-Based Sensor Sampling

Abstract

Systems with a rich array of sensors but limited power and/or processing resources have more potential information available than they can use and are forced to subsample the data. In this work, we build on observability analysis for general nonlinear systems to provide a basis for a framework to investigate dynamic sensor selection to optimize a measure of observability, specifically the condition number of an observability Gramian. This optimization is then applied to a sample system of natural Frenet Frames with sensing allowed to alternate between bearing and range measurements relative to a fixed beacon.

These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.

1 Introduction

Small autonomous vehicles can carry an array of simple, lightweight sensors; however, the ability to utilize the sensors is subject both to processing and to power constraints especially for sensors that have intensive power and processing needs. Inertial sensors, acoustic sensors (sonar), compasses, and cameras are commonly used in small aerial and underwater vehicles to provide estimates of the orientation and acceleration of the vehicle as well as local information, such as nearest objects and object recognition. To estimate global position, aerial vehicles typically rely on GPS; in indoor, underwater, or other GPS-denied environments, other methods are required, such as range and bearing measurements to a beacon at a known location. More complex tasks rely on the integration, or ‘fusion,’ of data from multiple sensors and multiple types of sensors. Given the constraints of power and computation time, the question arises as to how to best select among available sensors to optimize available resources while meeting information requirements.

To address this question, we draw inspiration from biological sensing. Biological species collect vast quantities of sensory input that is filtered and prioritized based on the potential information that it can supply; for example, one species of bat uses both vision and sonar, but sonar signals become more frequent as the light levels drop (when vision becomes less informative) and during landing (when more precise distance estimates are required) [1]. From this biological example, we develop here a framework for subsampling sensors based on the information they can provide about the state of the system.

The ability of a system to uniquely determine its state from the available sensor readings is characterized by its observability. In contrast to linear systems, observability of a nonlinear system may be affected by its trajectory, and thus by the particular control input history. Further, observability in nonlinear systems may be a local property with some areas of the state space being more or less observable for a given set of sensors and control inputs. Whether or not a system is observable at a given state can be determined by examining whether or not an invertible relationship can be found between the measurement and control histories and the desired states. In many cases, both linear and nonlinear, the question of observability can be addressed locally by generating an appropriate observability matrix (e.g., through linearization) and checking for sufficient rank. Hermann and Krener [2] presented a method for evaluating observability of continuous-time nonlinear systems, and Albertini and D’Alessandro [3] extended the theory to discrete-time nonlinear systems, finding a similar rank condition on the co-distribution of the observation space. Related extensions have also been developed in hybrid systems addressing conditions under which switched systems are observable [4,5,6].

These rank conditions generally provide a binary measure of observability; one can determine how ‘well’ a system can be observed by using a metric on a scalar-valued function of the information contained in the observability matrix. A common function used for metric-based observability is the observability Gramian. In order to construct an observability Gramian for a nonlinear system, the choices are to linearize around a viable trajectory and use the linear time-varying observability Gramian [7] or to employ the empirical observability Gramian, which calculates the output energy from perturbations of the initial state [8]. The latter provides a practical method of computing output energy, as in Scherpen’s work on balancing nonlinear systems [9], but has more recently found application in studying the observability of nonlinear systems [10] with the distinct advantage that an analytic solution is not required: the method only requires the ability to simulate the system and its outputs.

These tools have been well utilized to study autonomous vehicles , both in proving basic observability and in going a step further to design trajectories or system structure to optimize observability. Discrete-time observability analysis has been performed and observers developed for micro underwater vehicles based on a six degree of freedom (DOF) system model with intermittent beacon range-only measurements augmented by inertial measurements and depth sensors for dead reckoning [11]. The observability of nonholonomic integrators, a class of nonlinear systems, was studied, and the coupling of control and observability was exploited to optimize a measure of the system observability through choice of controls [12]. The sensor placement problem has also been approached using observability analysis to choose optimal sensor locations and types [13].

While sensor placement provides a static answer to observability optimization , a sensor scheduling approach allows for a dynamic subset of available sensors to provide measurements at each time step. Specifically, we are interested in the scheduling problem of selecting from a variety of sensors that provide unique types of sensor data and that are co-located on a moving platform. This problem is studied for a linear time-varying system with linear measurements in [14] and has similar goals and challenges to the more widely studied problem of determining how to best employ a distributed array of networked sensors to measure something in the external environment [15,16,17] such as tracking a target [18]. One of the larger challenges for these optimal observability problems is the rapidly increasing dimensionality as a function of the number of sensor options (e.g., type, number, possible locations), cost function, number of system states, and number and type of system controls: except for a subset of nicely posed systems (for example, linear systems with quadratic cost functions), finding the exact optimal solution to a particular problem requires an exhaustive tree-search of all possible sensor schedules [15].

In this work, we construct and provide a preliminary study of a framework to address the question of sensor scheduling. We are particularly interested in the application of these methods to nonlinear systems . A comparison is provided for the difference between the results for the Gramian of a linearization of the system dynamics about a valid, nominal trajectory and for the use of the empirical Gramian for the original nonlinear system. To ground the results in likely physical applications, we demonstrate the study for the Frenet frame model of motion of a vehicle in 3D space. The Frenet frame model is a version of the nonholonomic integrator, a canonical nonlinear system model for SE(3) that represents many real systems, such as aircraft, surface water vessels, and underwater vehicles. With this system structure, we incorporate sensors for range and bearing relative to a fixed and known beacon and address the optimal selection of the next sensor measurement. A range of trajectories with different radii of curvature and placement relative to the beacon are considered, and the resulting sensor schedules are compared.

The work here is organized as follows. Section 2 provides an overview of the methods used for determining the observability of general dynamic systems, our optimization problem is introduced in Sect. 3, followed by the introduction, observability analysis, and optimization results for an example system in Sect. 4. Finally, we discuss the results of our initial study of this type of sensor scheduling problem and the future work that we plan to do in Sect. 5.

2 Observability

While all digital systems have continuous-time dynamics and discrete-time measurements, often the update rate of the measurements is fast enough to reasonably use a continuous-time framework for both the dynamics and measurements. Given this assumption, a great deal of the work in nonlinear observability analysis has addressed continuous-time systems. Here, we are specifically interested in switching the sensors on and off, so a discrete-time framework is more relevant. To facilitate the discussion below, both the familiar continuous-time version of observability and the related discrete-time version are presented briefly. In either case, we are concerned with the ability to distinguish the initial state from other states given a set of known controls and measurements.

To begin, we will denote discrete-time system dynamics as

$$\begin{aligned} \Sigma _{d} : \begin{array}{ll} \mathbf {x}[k + 1] = \mathbf {f}(\mathbf {x}[k], \mathbf {u}[k]), &{} \quad k = 0,1,2, \ldots \\ \mathbf {y}[k] = \mathbf {h}(\mathbf {x}[k]), &{} \end{array}\nonumber \end{aligned}$$

and continuous-time system dynamics as

$$\begin{aligned} \Sigma _{c} : \begin{array}{ll} \dot{\mathbf {x}}(t) = {\tilde{\mathbf {f}}}(\mathbf {x}(t), \mathbf {u}(t))\\ \mathbf {y}(t) = \mathbf {h}(\mathbf {x}(t)) \end{array} \nonumber \end{aligned}$$

where the states $\mathbf {x}[k],\mathbf {x}(t) \in M$ and measurements $\mathbf {y}[k],\mathbf {y}(t) \in Y$, respectively lie within connected, differentiable manifolds $M \subset \mathbb {R}^n$ and $Y \subset \mathbb {R}^p$, and the controls $\mathbf {u}[k],\mathbf {u}(t) \in U$ lie within connected, differentiable manifold $U \in \mathbb {R}^m$. In cases where a result applies to both $\Sigma _d$ or $\Sigma _c$, the subscript will be dropped for brevity. The following definitions in [3] regarding observability will be employed here.

Definition 1

Indistinguishable states $x_1, x_2 \in M$, denoted $x_1 I x_2$, will produce identical outputs for any identical combination of controls. A state $x_1 \in M$ is observable if being indistinguishable from state $x_2$ implies $x_1 = x_2$ for all $x_2 \in M$. The system $\Sigma$ is observable if $x_1 I x_2 \implies x_1=x_2 ~\forall ~ x_1,x_2 \in M$.

Definition 2

State $x_0 \in M$ is locally weakly observable if there exists some neighborhood W of $x_0$ such that for each $x_1 \in W$, $x_0Ix_1 \implies x_0 = x_1$. A system $\Sigma$ is locally weakly observable if all states $x_0$ are locally weakly observable.

Definition 3

State $x_0 \in M$ is locally strongly observable if there exists some neighborhood W of $x_0$ such that for each $x_0, x_1 \in W$, $x_0Ix_1 \implies x_0 = x_1$. A system $\Sigma$ is locally strongly observable if all states $x_0$ are locally strongly observable.

2.1 Analytical Observability

The underlying principle for determining analytical observability is the same for continuous and discrete-time systems: check for the existence of a relationship between the outputs, inputs, and states that can be inverted to uniquely determine the initial state from the outputs and inputs. The outcomes of this type of study provide insight into the requirements for control function structure and limitations on allowable trajectories or sensor placement to enable full observability. In either the continuous or discrete scenario, the process can be locally reduced to a rank condition on a matrix composed of either a sequence (discrete) or the derivatives (continuous) of the outputs.

2.1.1 Continuous-Time Systems

Assume that $\Sigma _c$ is nonlinear in the states, $\mathbf {x}(t)$, linear in the control inputs, $u_i(t)$, and that it can be written in the control affine form

$$\begin{aligned} \dot{\mathbf {x}}(t) = {\tilde{\mathbf {f}}}_0(\mathbf {x}(t)) + \sum _{i=1}^{m} {\tilde{\mathbf {f}}}_i(\mathbf {x}(t)) u_i(t) = {\tilde{\mathbf {f}}}(\mathbf {x}(t),\mathbf {u}(t)), \nonumber \end{aligned}$$

where ${\tilde{\mathbf {f}}}_0$ is termed the drift, and ${\tilde{\mathbf {f}}}_i$ are termed the control vector fields. The collection of terms forming the observability space, $\mathscr {O}_c$, are constructed from the time derivatives of the output functions:

$$\begin{aligned} \frac{d}{dt} \mathbf{h}= & {} \left( \frac{\partial }{\partial x} \mathbf {h} \right) {\tilde{\mathbf {f}}}(\mathbf {x}(t),\mathbf {u}(t)) \nonumber \\ \frac{d}{dt^l} \mathbf{h}= & {} \frac{d}{dt} \left( \frac{d}{dt^{l-1}} \mathbf {h} \right) , ~l\in {\mathbb N}. \nonumber \end{aligned}$$

These time derivatives can be equivalently represented using Lie derivatives, defined as

$$\begin{aligned} L_{\tilde{f}_i} \mathbf {h} = \frac{\partial \mathbf {h}}{\partial \mathbf {x}} {\tilde{\mathbf {f}}}_i, \nonumber \end{aligned}$$

with repeated and mixed derivatives respectively calculated as

$$\begin{aligned} L_{\tilde{f}_i}^k = \frac{\partial L_{\tilde{f}_i}^{k-1}\mathbf {h}}{\partial \mathbf {x}} {\tilde{\mathbf {f}}}_i \quad \text { and } \quad L_{\tilde{f}_j} L_{\tilde{f}_i} = \frac{\partial (L_{\tilde{f}_j}\mathbf {h})}{\partial \mathbf {x}} {\tilde{\mathbf {f}}}_i, \nonumber \end{aligned}$$

the latter arising from use of switching (area generating) controls captured by the Lie bracket of the vector fields $[{\tilde{\mathbf {f}}}_i,{\tilde{\mathbf {f}}}_j] = (\partial {\tilde{\mathbf {f}}}_j/\partial \mathbf {x}) {\tilde{\mathbf {f}}}_i - (\partial {\tilde{\mathbf {f}}}_i/\partial \mathbf {x}) {\tilde{\mathbf {f}}}_j$. The observability space of $\Sigma _c$, $\mathscr {O}_c$, is thus also the span of the Lie derivatives with respect to the drift and control vector fields, that is $\mathscr {O}_c$ = span$\{\mathbf {h}, L_{\tilde{f}_i} \mathbf {h}, \dots \}$, and is alternately termed the observability Lie algebra. Determining whether the relationship between the terms in the observability space, $\mathscr {O}_c$, and the states can be inverted is generally analytically intractable, but one can address the question locally using the inverse function theorem and the local co-distribution of the observability matrix. If $\text {d}\mathscr {O}_c$, the co-distribution of $\mathscr {O}_c$ at a point in the state space $\mathbf {x}_0$, is full rank, then the system is locally weakly observable at that point [2].

If the system is linear time-invariant with ${\tilde{\mathbf {f}}}(\mathbf {x}(t)) = \tilde{A} \mathbf {x}(t) + \tilde{B} \mathbf {u}(t)$ and $\mathbf {y} = \mathbf {h}(\mathbf {x}(t)) = C \mathbf {x}(t)$, then differentiating the output gives

$$\begin{aligned} \begin{bmatrix} \mathbf {y} \\ \mathbf {y}^{(1)} \\ \vdots \\ \mathbf {y}^{(n)} \end{bmatrix} = \begin{bmatrix} C \\ C\tilde{A} \\ \vdots \\ C \tilde{A}^{n-1} \end{bmatrix} \mathbf {x} + \begin{bmatrix} 0 \\ \tilde{B} \\ \vdots \\ C \tilde{A}^{n-2}\tilde{B} + \cdots + C\tilde{B} \end{bmatrix}\, \mathbf {u} = \mathscr {O}_{LTI} \mathbf {x} + \mathscr {U} \mathbf {u}. \end{aligned}$$

(1)

where superscripts in parentheses indicate time derivatives and $\mathscr {O}_{LTI}$ is the continuous-time observability matrix. As all terms other than the state are assumed known, if $\mathscr {O}_{LTI}$ is full rank, and thus has an invertible square submatrix, then the system is observable. Thus, in the linear case, regardless of the control action, observability is completely determined by the observability matrix.

2.1.2 Discrete-Time Systems

The conditions for discrete-time observability are presented for a single input-single output system by Albertini and D’Asessandro in [3]. Given the system $\Sigma _d$, let the system dynamics for a choice of control, $\mathbf {u} \in U$, be denoted $f_{u}(\mathbf {x}) = f(\mathbf {x},\mathbf {u})$. Then, define the output sequence, $\varTheta _1, \varTheta _2, ...$, at each time step, $k \ge 1$, as a function of the inputs as

$$\begin{aligned} \varTheta _1= & {} \{ \mathbf {h}(\cdot ) \} \nonumber \\ \varTheta _k= & {} \{ \mathbf {h}(f_{u_j} \circ \cdots \circ f_{u_1}(\cdot ) | \forall i = 1,\dots ,j, \mathbf {u}_i \in U, \text { and } 1 \le j \le k-1 \}, \end{aligned}$$

(2)

where $\circ$ denotes composition, and $\mathscr {O}_d = \cup _{k \ge 1} \varTheta _k$ is the collection of the measurement functions. As with continuous-time nonlinear systems , the inverse function theorem is generally employed to find a local inversion of this relationship between states, measurements, and controls. For system $\Sigma _d$ and state $\mathbf {x}_0$, the co-distribution of $\mathscr {O}_d$ at $\mathbf {x}_0$ is given by $d\mathscr {O}_d(\mathbf {x}_0)$. If the co-distribution is full rank, that is, rank($d\mathscr {O}_d(\mathbf {x}_0) )= n$, then the system is locally weakly observable at $\mathbf {x}_0$.

This condition naturally holds for linear(ized) systems as well but without the need to employ the inverse function theorem. Assume that $\Sigma _d$ can be written as the linear time-varying system

$$\begin{aligned} \Sigma _{d,LTV} : \begin{array}{ll} \mathbf {x}[k + 1] = A[k]\mathbf {x}[k] + B[k]\mathbf {u}[k], &{} \quad k = 0,1,2, \ldots \\ \mathbf {y}[k] = C[k]\mathbf {x}[k]; &{} \end{array} \nonumber \end{aligned}$$

then the measurements in the sequence defined in (2) can be written as

$$\begin{aligned} \mathbf {y}[k] = C[k] \Big (\varPhi _d[k,0] \mathbf {x}[0] + \sum _{i=1}^{k-1} \varPhi _d[k, i+1] B[i] u[i]\Big ), \nonumber \end{aligned}$$

where the discrete state transition matrix is defined as

$$\begin{aligned} \varPhi _d[k_f,0] = \prod _{k=0}^{k_f-1} A[k]. \end{aligned}$$

(3)

In the time-sinvariant case, $\varPhi _d[k_f,0]$ simplifies to $A^{k_f}$, and the sequence of measurements compiled into the discrete-time linear time-invariant observability matrix is the same form as the continuous case, $\mathscr {O}_{LTI}$, defined in (1).

2.2 Observability Gramians

While the above analysis can determine whether or not a system is observable, it does not provide any quantitative characteristics of observability. An alternative approach to observability that admits the use of a metric is that of the observability Gramian. In particular, by incorporating structural or controlled parameters into the system description, these metrics can be used to determine best parameter choice for maximal observability.

2.2.1 Linear Observability Gramian

The traditional form of the observability Gramian applies to linear systems. The discrete-time observability Gramian, $W_{o,d}$, is defined for a linear, time-varying system as

$$\begin{aligned} W_{o,d}[k_0,k_f] = \sum _{k = k_0}^{k_f} \varPhi _d^T[k,k_0] C^T[k] C[k] \varPhi _d[k,k_0], \end{aligned}$$

(4)

where $\varPhi _d[k,0]$ is defined as in (3). The continuous-time observability Gramian, $W_{o,c}$, is constructed using the continuous-time state transition matrix, $\varPhi _c(t,t_0)$, as

$$\begin{aligned} W_{o,c}(t_0,t_f) = \int _{t = t_0}^{t_f} \varPhi _c^T(t,t_0) C_c^T(\mathbf {x}^0(t)) C_c(\mathbf {x}^0(t)) \varPhi _c(t,t_0) dt, \end{aligned}$$

(5)

where $\varPhi _c(t,t_0)$ satisfies

$$\begin{aligned} \nonumber \frac{\partial }{\partial t}\varPhi _c(t,t_0 ) = \tilde{A}(\mathbf {x}(t), \mathbf {u}(t)) \varPhi _c(t,t_0). \end{aligned}$$

If the observability Gramian, $W_{o,d}[k_0,k_f]$ or $W_{o,c}(t_0,t_f)$, is full rank, n, for some time step $k_f$ or $t_f$, then the linear time-varying system is observable. Measures of observability based on the Gramian follow in section “Measures of Nonlinear Observability”.

The observability Gramian $W_{o,c}$ can be used to study the observability of the nonlinear system $\Sigma _c$ by linearizing the system about some nominal trajectory, $(\mathbf {x}^0(t), \mathbf {u}^0(t))$, using the first term of a Taylor series expansion of ${\tilde{\mathbf {f}}}(\mathbf {x}(t),\mathbf {u}(t))$ evaluated along the trajectory to get the linear, time-varying system

$$\begin{aligned} \Sigma _{c,LTV} : \begin{array}{ll} \dot{\bar{\mathbf {x}} } &{}= A_c(\mathbf {x}^0(t), \mathbf {u}^0(t)) \bar{\mathbf {x}}(t) + B_c(\mathbf {x}^0(t), \mathbf {u}^0(t)) \bar{\mathbf {u}}(t) \\ \bar{\mathbf {y}} &{}= C_c(\mathbf {x}^0(t))\bar{\mathbf {x}}(t), \end{array} \nonumber \end{aligned}$$

where  ${^\text {-}}$  denotes deviation from the nominal trajectory and

$$\begin{aligned} A_c(\mathbf {x}(t), \mathbf {u}(t))= & {} \left. \frac{\partial {\tilde{\mathbf {f}}}(\mathbf {x}(t), \mathbf {u}(t))}{\partial \mathbf {x}} \right| _{\mathbf {x}(t) = \mathbf {x}^0(t), \mathbf {u}(t) = \mathbf {u}^0(t)} \nonumber \\ B_c(\mathbf {x}(t), \mathbf {u}(t))= & {} \left. \frac{\partial {\tilde{\mathbf {f}}}(\mathbf {x}(t), \mathbf {u}(t))}{\partial \mathbf {u}} \right| _{\mathbf {x}(t) = \mathbf {x}^0(t), \mathbf {u}(t) = \mathbf {u}^0(t)} \nonumber \\ C_c(\mathbf {x}(t), \mathbf {u}(t))= & {} \left. \frac{\partial \mathbf {h}(\mathbf {x}(t))}{\partial \mathbf {x}} \right| _{\mathbf {x}(t) = \mathbf {x}^0(t)}. \nonumber \end{aligned}$$

A linear, time-varying discrete-time approximation of $\Sigma _c$ can be produced by using the matrix exponential to calculate the discrete state space matrix $A_d[k] = \exp \{ A_c(k \varDelta _T) \}$, where $\varDelta _T$ is the size of the time step, which is in turn used to calculate $\varPhi _d[k,0]$ from (3).

2.2.2 Empirical Observability Gramians

The linear Gramians can provide a basis for analysis and optimization of nonlinear systems; however, it is important to remember that the linear approximations do not capture all of the dynamics of the nonlinear system and thus do not always provide accurate results. It is not uncommon that a linearized system would be classified as unobservable when the original nonlinear system in fact observable. To demonstrate this phenomenon, consider the scalar system $\dot{x} = u$ with measurement function $y = \cos (x)$. Assuming nonzero controls, the nonlinear observability co-distribution will be $d \mathscr {O} =$span$\{ \cos (x), \sin (x)\}$ which will always have a dimension of 1 and thus be observable. The linearized measurement function $\bar{y} = -\sin (x)|_{x^0}$ will render the system unobservable when $x^0 = 2 \pi k$. Also, in the linearized system, the controls are predetermined along the nominal trajectory, and thus cannot be used for analysis in the same way as in the original nonlinear structure.

To extend the use of observability Gramians to nonlinear systems , Lall, Marsden, and Glavaški introduced the empirical observability Gramian [8], which uses perturbations, $\varepsilon \mathbf {e}_i$, of the initial state, $\mathbf {x}_0 = \mathbf {x}(0)$, to quantify changes in the output measurements. The original version did not include the control explicitly, so instead we use the continuous-time empirical observability Gramian from [19]:

$$\begin{aligned} W_{c}^{\varepsilon }(t_f,\mathbf {x}_0,\mathbf {u}^0(t)) = \frac{1}{4 \varepsilon ^2} \int _{0}^{t_f} \varPhi ^{\varepsilon }(t,\mathbf {x}_0,\mathbf {u}^0(t))^T \varPhi ^{\varepsilon }(t,\mathbf {x}_0,\mathbf {u}^0(t))dt. \nonumber \end{aligned}$$

Further work by Powel and Morgansen [20] defines the discrete-time version as

$$\begin{aligned} W_{d}^{\varepsilon }[k_f,\mathbf {x}_0,\mathbf {u}^0[k]] = \frac{1}{4 \varepsilon ^2} \sum _{k = 0}^{k_f} \varPhi ^{\varepsilon }[k,\mathbf {x}_0,\mathbf {u}^0[k]]^T \varPhi ^{\varepsilon }[k,\mathbf {x}_0,\mathbf {u}^0[k]] \end{aligned}$$

(6)

where $\varPhi ^{\varepsilon }(t,\mathbf {x}_0,\mathbf {u}^0(t))$ and $\varPhi ^{\varepsilon }[k,\mathbf {x}_0,\mathbf {u}^0[k]]$ are defined similarly by perturbations of the output in each direction at each time (step). The discrete-time version is given as

$$\begin{aligned} \nonumber \varPhi ^{\varepsilon }[k,x_0,u] = \begin{bmatrix} y^{+1} - y^{-1}&\cdots&y^{+n} - y^{-n} \end{bmatrix} \end{aligned}$$

with $y^{\pm i} = y[k, x_0 \pm \varepsilon \hat{\mathbf {e}}_i, u]$ where $\hat{\mathbf {e}}_i$ denote elements of the standard orthonormal basis in $\mathbb {R}^n$, and the magnitude of the perturbation, $\varepsilon$, is chosen relative to the magnitude of the states; in the subsequent example, $\varepsilon = 0.01$ (a reasonable choice for states with magnitude on the order of one [10]).

2.2.3 Measures of Nonlinear Observability

Eigenvalues of an observability Gramian provide measures of observability of the system [10]. The maximum eigenvalue, $\lambda _1(W)$, gives an idea of how much output energy is in the most observable state. Similarly, the minimum eigenvalue $\lambda _n(W)$, gives an idea of how much output energy is in the least observable state. For a well-conditioned estimate, it is desirable to have similar energy in the most and least observable states so that a small change in one state does not overwhelm changes in another state. This relation is captured by the condition number, $\kappa (W) = \lambda _1(W) /\lambda _n(W)$ that takes a minimum value of unity. It is worthwhile to note that each of these measures can be characterized as convex functions of the observability [21].

One variation on the condition number which will prove useful here is defining it using the largest and smallest singular values instead of eigenvalues:

$$\begin{aligned} \kappa (W) = \sigma _1(W) /\sigma _3(W). \end{aligned}$$

(7)

This change accommodates systems with constraints built into their definitions that cause some of the eigenvalues of the Gramian to have value zero (since the observability Gramian is positive definite by definition, the smallest eigenvalue would be zero, rendering the standard condition number useless).

3 Switched Sensing Optimization

Given a set of sensors with sufficient resources to use only a subset of the available sensors or their data at a time, our goal here is to use measures of observability to determine which sensor(s) should be used at any point in time to maximize observability. We begin by introducing a sensor selection switch into the discrete system description, $\Sigma _d$. Specifically, assume that a set of p sensors is available, each of which is respectively described by the measurement function, $y_i, ~i \in \{1, \dots , p\}$. For each sensor, incorporate a binary-valued switch function $s_i \in \{0,1\}$ where $s_i=1$ if the sensor is active, and $s_i=0$ if the sensor is inactive. The measurement functions for the system at each time step, k, can then be described by

$$\begin{aligned} \nonumber y_i[k] = s_i[k] h_i[k], ~ i \in \{1, \dots , p\}, ~ s_i[k] \in \{0,1\}. \end{aligned}$$

To restrict the number of active sensors to $p_k$, the constraint $\sum _{i=1}^p s_i[k]=p_k$ would be applied at each time step. Alternatively, $p_k$ could represent some resource budget and $c_i$ could represent the relative resource usage of sensor i; then the constraint would be $\sum _{i=1}^p c_i s_i[k] \le p_k$.

At each time step we then seek to choose $s_i[k]$ to optimize some preferred measure of observability, J(s), subject to the constraints. Stated in the simplest form, this problem is written as

$$\begin{aligned} \min _{s}&J(s) \\ \text {subject to }&s_i[k] \in \{0,1\} \quad \forall i,k \nonumber \\&\sum _{i=1}^p c_i s_i[k] \le p_k \quad \forall k. \nonumber \end{aligned}$$

(8)

The cost function, J(s), could be one of the convex measures described in the previous section. The constraints require that a sensor be either on or off (unfortunately this constraint is not convex) and that a maximum number of sensors is not exceeded at any time step.

3.1 Finite Look-Ahead Optimization

For the current work, we introduce a model predictive control type scheme to find the exact solution to locally optimize the mixed-integer program for choosing one of two sensors ($p=2$ and $p_k = 1$ for all k) with a look-ahead of N time steps. We employ this method rather than calculating over an entire trajectory because the computations required to calculate each option N steps for even the simplest case of two sensors grows as $2^N$. This optimization becomes infeasible for even moderately large N or for larger numbers of sensors. To determine the optimal sensor selection for time step $k=K+1$, the empirical and linear Gramians are calculated with data from $k=1$ to K with the optimal sensor sequence and then from $K+1$ to $K+1+N$ with each of the potential sensor sequences. The sequence with the minimum condition number at $K+N+1$ is chosen, and the first sensor (or subset of sensors) in the sequence is selected for time $K+1$. Thus, (8) becomes

$$\begin{aligned} \min _{s[k], k =K+1,\dots ,K+N+1}&J( W^*[K+N+1], s) \\ \text {subject to } \quad&s_i[k] = \{0,1\}, \quad i = 1,\dots ,p,\text { and } k=1,\dots ,K+N+1 \nonumber \\&\sum _{i=1}^p s_i[k] = 1, \quad k=1,\dots ,K+N+1 . \nonumber \end{aligned}$$

(9)

where $W^*[K+N+1]$ is either the linear Gramian, $W_{o,d}[0, K+N+1]$, defined in (4) over a nominal trajectory $(\mathbf {x}^0(t), \mathbf {u}^0(t))$ or the empirical Gramian, $W_d^{\varepsilon }[K+N+1, \mathbf {x}_0, \mathbf {u}^0[k]]$, defined in (6) calculated from numerical simulation. In other words: at each time step K, the sensor sequence is optimized over a finite look-ahead, the first sensor in that sequence is selected for the next time step $K+1$, and the process is repeated now incorporating the $K+1$ measurement, as outlined in Algorithm 1.

3.2 Convex Relaxation

To allow for faster solutions, the mixed-integer optimization (8) or (9) could be relaxed to a convex optimization framework by allowing the sensors to be ’partially’ on during a given time step; to promote sparsity in s (and thus that sensors are either on or off), an $\ell _1$ norm can be added to the cost function [21, 22]. With those modifications, the optimization (8) would become

$$\begin{aligned} \min _{s}&J(s) + c\Vert s\Vert _1 \nonumber \\ \text {subject to}&0 \le s_i[k] \le 1 \quad \forall i,k \nonumber \\&\sum _{i=1}^p c_i s_i[k] = 1 \quad \forall k. \nonumber \end{aligned}$$

The constant scalar, c, in the cost function allows weighting the importance of sparsity of s versus minimization of J(s).

4 Application to Frenet–Serret Frames

Frenet–Serret and natural Frenet frames both describe the evolution of an orthogonal coordinate system along a trajectory [23]. In previous work, the same authors use natural Frenet frames to design vehicle trajectories for a variety of tasks, such as pursuit, formation flight, and boundary tracking (see references in [23]). The direction of motion along a smooth path is defined as the tangent $\mathbf {T}(s) = \gamma '(s)$, where $\gamma (s)$ is the trajectory normalized along its arc s such that $\gamma '(s)$ is a unit vector ($|\gamma '(s)|$ = 1). Frenet–Serret frames define the additional two vectors to make up an orthogonal frame as the normal, $\mathbf {N}(s)$, perpendicular to the motion in the plane of the curve, and the binormal, $\mathbf {B}(s)$, which completes the set. The evolution of the ($\mathbf {T}, \mathbf {N}, \mathbf {B}$) frame is given by the curvature and torsion of the curve. A drawback to this formulation is that the frame is not well defined when the second derivative of $\gamma (s)$ is zero. This constraint motivates the use of a modified version of the Frenet–Serret frames termed the natural Frenet frame, which we will denote by the orthogonal vector set $(\mathbf {t}, \mathbf {n}, \mathbf {b})$. In the natural Frenet structure, the tangent vector is defined as in the Frenet–Serret frames, $\mathbf {t} = \mathbf {T}$, and the remaining vectors are defined to be orthogonal unit vectors that complete the SO(3) group element (see Fig. 1).

To utilize the natural Frenet frames to describe motion of a vehicle, note that the ‘particle’ model of a vehicle that cannot move directly perpendicular to its current velocity can be written in the following form:

$$\begin{aligned} \mathbf {r}'= & {} \mathbf {t} \\ \mathbf {t}'= & {} \mathbf {n} u_1 + \mathbf {b} u_2 \nonumber \\ \mathbf {n}'= & {} -\mathbf {t} u_1 \nonumber \\ \mathbf {b}'= & {} -\mathbf {t} u_2 \nonumber \end{aligned}$$

(10)

where $'$ indicates differentiation with respect to arc length, s, or with respect to time, t, if we assume unit velocity. The controls, $u_1$ and $u_2$, affect the curvature of the path in the $\mathbf {t}(t)-\mathbf {n}(t)$ and $\mathbf {t}(t) - \mathbf {b}(t)$ planes, respectively.

The measurement model considered for this example is a beacon located at the origin, as depicted in Fig. 1, that provides range and/or bearing measurements of the system according to

$$\begin{aligned} h_r(\mathbf {x})= & {} \frac{1}{2}(r_1^2 + r_2^2 + r_3^2) = \frac{1}{2}\Vert \mathbf {r}\Vert _2^2 \end{aligned}$$

(11)

$$\begin{aligned} h_b(\mathbf {x})= & {} \arctan \left( \frac{r_2}{r_1} \right) \end{aligned}$$

(12)

where we take the full state vector to be $\mathbf {x} = \begin{bmatrix} \mathbf {r}^T&\mathbf {t}^T&\mathbf {n}^T&\mathbf {b}^T \end{bmatrix}^T$.

Fig. 1

Diagram showing the $\mathbf {t}-\mathbf {n}-\mathbf {b}$ Frenet frame along a curve, with position denoted in the global reference frame by $\mathbf {r}$ and the bearing angle $\beta$

In discrete time, assuming a sufficiently small time step, $\varDelta _T$, the dynamics of transitioning from step k to step $k+1$ can be approximated as

$$\begin{aligned} \mathbf {x}[k+1] = \mathbf {x}[k] + \varDelta _T \Big (\mathbf {f}_0 (\mathbf {x}[k]) + \mathbf {f}_1(\mathbf {x}[k]) u_{1}[k] + \mathbf {f}_2(\mathbf {x}[k]) u_{2}[k] \Big ) \end{aligned}$$

(13)

where $\mathbf {f}_1 = \begin{bmatrix} \mathbf {0}^T&\mathbf {n}^T&-\mathbf {t}^T&\mathbf {0}^T \end{bmatrix}^T$ and $\mathbf {f}_2 = \begin{bmatrix} \mathbf {0}^T&\mathbf {b}^T&\mathbf {0}^T&-\mathbf {t}^T \end{bmatrix}^T$, $\mathbf {0}$ denoting a vector of zeros the size of $\mathbf {t}$. It is important to note that while the discrete- and continuous-time systems have a total of twelve variables, the nine variables from the $(\mathbf {t},\mathbf {n},\mathbf {b})$ frame have only three degrees of freedom (three dimensional attitude), so the full system has only six independent states. Thus, when performing observability analysis, we do not expect to obtain rank $n = 12$ matrices but rather $n_{DOF}=6$. When constrained to a plane, the reduced system has six variables with three degrees of freedom and three states.

4.1 Analytical Observability Analysis

Using the discrete approximation of the dynamics (13) and measurement functions (11) and (12), we initially address the observability of the system by considering a drift only scenario with no controls by calculating the first few time steps of the dynamics to construct $\varTheta$ defined in (2). With $\mathbf {u} = \mathbf {0}$, we have the state and measurement functions

$$\begin{aligned} \mathbf {x}[k]= & {} \begin{bmatrix} (\mathbf {r}_0^T + k \varDelta _T \mathbf {t}_0^T)&\mathbf {t}_0^T&\mathbf {n}_0^T&\mathbf {b}_0^T \end{bmatrix}^T \nonumber \\ h_r(\mathbf {x}[k])= & {} \frac{1}{2} \Vert \mathbf {r}_0 + k \varDelta _T \mathbf {t}_0 \Vert _2^2 \nonumber \\ h_b(\mathbf {x}[k])= & {} \arctan \left( \frac{\mathbf {r}_{0,2} + k \varDelta _T}{\mathbf {r}_{0,1} + k \varDelta _T} \right) \nonumber \end{aligned}$$

where the zero subscript indicates the initial state, e.g. $\mathbf {r}_0 = \mathbf {r}(0)$. Investigating the range and bearing measurement functions individually, we take the appropriate directional derivatives to build the co-distributions for range-only and bearing-only measurements:

$$\begin{aligned} d\varTheta _r= & {} \begin{bmatrix} \mathbf {r}_0^T&\mathbf {0}&\mathbf {0}&\mathbf {0} \\ \mathbf {r}_0^T + \varDelta _T \mathbf {t}_0^T&\varDelta _T (\mathbf {r}_0^T + \varDelta _T \mathbf {t}^T)&\mathbf {0}&\mathbf {0} \\ \vdots&\vdots&\vdots&\vdots \\ \mathbf {r}_0^T + k\varDelta _T \mathbf {t}_0^T&k\varDelta _T (\mathbf {r}_0^T + k\varDelta _T \mathbf {t}^T)&\mathbf {0}&\mathbf {0} \end{bmatrix} \nonumber \\ d\varTheta _b= & {} \begin{bmatrix} \frac{ -\mathbf {r}_{0,2} }{ \mathbf {r}_{0,1}^2 + \mathbf {r}_{0,2}^2 }&\frac{ \mathbf {r}_{0,1} }{ \mathbf {r}_{0,1}^2 + \mathbf {r}_{0,2}^2 }&0&0&0&0&\mathbf {0}&\mathbf {0} \\ -\frac{\mathbf {r}_{0,2} + \varDelta _T \mathbf {t}_{0,2}}{ c(1)}&\frac{\mathbf {r}_{0,1} + \varDelta _T \mathbf {t}_{0,1}}{c(1)}&0&\frac{\varDelta _T (\mathbf {r}_{0,2} + \varDelta _T \mathbf {t}_{0,2}) }{c(1)}&\frac{\varDelta _T (\mathbf {r}_{0,1} + \varDelta _T \mathbf {t}_{0,2}) }{c(1)}&0&\mathbf {0}&\mathbf {0} \\ \vdots&\vdots&\vdots&\vdots&\vdots&\vdots \\ -\frac{\mathbf {r}_{0,2} + k \varDelta _T \mathbf {t}_{0,2}}{ c(k )}&\frac{\mathbf {r}_{0,1} + k \varDelta _T \mathbf {t}_{0,1}}{c( k)}&0&\frac{k\varDelta _T (\mathbf {r}_{0,2} + k\varDelta _T \mathbf {t}_{0,2}) }{c(k)}&\frac{k\varDelta _T (\mathbf {r}_{0,1} + k\varDelta _T \mathbf {t}_{0,1}) }{c(k)}&0&\mathbf {0}&\mathbf {0} \end{bmatrix} \nonumber \\ \quad&c(k) = (\mathbf {r}_{0,1} + k\varDelta _T \mathbf {t}_{0,1})^2 + (\mathbf {r}_{0,2} + k \varDelta _T \mathbf {t}_{0,2})^2 \nonumber \end{aligned}$$

with $\mathbf {0}$ a vector of zeros of the same dimension as $\mathbf {r}_0^T$. The co-distribution generated from range-only measurements, $d\varTheta _r$, is not full rank; the first three columns and next three columns are linearly dependent on each other, so the maximum rank is three. Similarly, the rank of the co-distribution from bearing-only measurements, $d\varTheta _b$, is at most three. The system has six degrees of freedom, so with either sensor and no input, the system is unobservable; with both bearing and range measurements together, however, the maximum rank is six, and the system is observable except at singular points where the rank drops below six. These results are consistent with an analysis of the continuous-time system which is omitted for brevity.

4.2 Optimization with Observability Gramians

Knowing that the system of evolving natural Frenet frames discussed in the previous section is observable with some combination of measurement functions, we proceed to address the optimization problem (9). For clarity of presentation, we restrict this study to a planar version of the system and note that the results will generalize to the full system. Here, we used the condition number defined in (7). To avoid issues arising from rank deficiency of the Gramian due to insufficient measurement data, we initialized the optimization by providing both range and bearing sensors for $k=0$. This approach is not unreasonable, as it is plausible that an autonomous system would collect all possible sensor data before embarking on its mission. In all cases, the look-ahead was $N=5$ for both the empirical and linear Gramians, and Matlab was used to simulate the system dynamics and perform the optimization .

Some informative results from this initial study are provided here, beginning with the effect of $\varDelta _T$ on sensor selection. This quantity can be interpreted as the sensor update rate and was held constant for any given trajectory simulation. The optimal sensor selection for a range of $\varDelta _T$ from 0.25 to 1 for a circular path generated by a constant control of $u_1 = 0.25$ ($u_2 = 0$ for the planar case) is shown in Fig. 2. The optimization results from both the empirical observability Gramian and the linear observability Gramian tended to result in more use of bearing measurements, with range measurements selected more frequently near the initial location and for a couple of outliers elsewhere on the trajectory. The condition numbers calculated for both optimization approaches showed similar trends of an initial decrease in condition number as the trajectory traced the first half of a circle, with values for most test cases stabilizing around the first full revolution.

Based on the geometry of the trajectories in Fig. 2 with respect to the beacon, one might hypothesize that the clustering of range measurements was relative to location within the $r_1-r_2$ plane. The sensor selection results in Figs. 3 and 4, however, show that the geometry of the trajectory with respect to its own start point plays a larger role. In all cases, there is a cluster of range measurements near the initial location not only at the beginning of the trajectory, but also on subsequent returns to the initial point. The range measurement is more often used for the trajectories with larger radius of curvature. The condition numbers (see Fig. 4) show a general decreasing trend that is somewhat cyclic around the circle.

The sensor selection results for other periodic (but noncircular) trajectories have similar characteristics, as seen in Fig. 5. As the trajectory exhibits more extensive changes in curvature, the range measurements are used more frequently. The trend is more apparent in the empirical Gramian optimization, however, it is present in the linear Gramian optimization as well. The condition number for the empirical observability Gramian maintains a relatively constant mean after an initial decrease. The condition number for the linear observability Gramian is much smoother, and shows that the plots with lower curvature have more poorly conditioned estimates as one would expect based on the work done in [12].

Fig. 2

Trajectories (top), sensor selection (middle), and condition number with markers for bearing and range measurements (blue triangles and red circles) to the beacon at the origin (black circles), chosen to minimize the condition number of the empirical (left), and linear (right) observability Gramian. A constant control $u_1 = 0.25$ was applied while varying the time step, $\varDelta _t= \{0.25, 0.5, 0.75, 1\}$. The x-axis for the condition number plots is the angle $\alpha [k]$, measured counterclockwise from the center of the circle trajectory between the initial location (black square) and $\mathbf {r}[k]$

Fig. 3

Trajectories with markers for bearing and range measurements (blue triangles and red circles) to the beacon at the origin, chosen to minimize the condition number of the empirical, or linear observability Gramian. The time step was $\varDelta _T=0.5$ for all trajectories. Each trajectory represents a constant control from $u = \{0.1875, 0.25, 0.375, 0.5 \}$ (largest circle to smallest circle) and the initial state is noted by a black square

Fig. 4

Condition number of the empirical (top four plots) or linear (bottom four plots) observability Gramians, corresponding to the trajectories shown in Fig. 3. The time step was $\varDelta _t=0.5$ for all trajectories. Each trajectory represents a single control from $u = \{0.1875, 0.25, 0.375, 0.5\}$, going from small (blue) to large (red). The x-axis for the condition number plots is the angle $\alpha (k)$, measured counterclockwise from the center of the circle trajectory between the initial location (black square) and $\mathbf {r}[k]$

Fig. 5

Trajectories (top) and condition number (bottom) with markers for bearing and range measurements (blue triangles and red circles) to the beacon at the origin, chosen to minimize the condition number of the empirical or linear observability Gramian. The time step was $\varDelta _t=0.25$ with control $u = u_A \cos (k \varDelta _t /2)$, $u_A = \{0, 0.5, 1, 1.5 \}$

5 Conclusions

We have developed a framework for approaching the sensor selection problem in nonlinear systems and provided some interesting initial results for a Frenet frame motion model with beacon range and bearing measurements. These results indicate that while relative location to the beacon does play a role in sensor selection, the time history of the trajectory has a greater impact on the optimal sensor selection (at least for a look-ahead of $N=5$). The results support the fact that the choice of trajectory influences the observability and that curvier paths provide better conditioned observability Gramians.

The framework developed here can be used to address some questions about nonlinear observability that will have direct impact on effective design of autonomous vehicles and their controllers. The first set of results (Fig. 2) indicate that sensor sampling rates affect the observability of the system, but further study can be done to investigate how the sampling rate affects the system and then how to use that information to help choose the appropriate sensor. Knowing that control choice, and thus trajectory, influences the observability, we are working to formalize the relationship between path history and optimal sensor choice, particularly with respect to cyclical trajectories and how the history built into the data patterns brings about the repeated selection of certain sensor types. In addition to the history, the effect of longer look-ahead and comparison of local and global results should be performed. Related to these characteristics is the effect of deviating from the planned look-ahead on the conditioning of the estimate. Sensor fidelity and cost of use as well as task-specific requirements will also be incorporated.