Optimal Control with Limited Sensing via
Empirical Gramians and Piecewise Linear Feedback
Atiye Alaeddini
Institute for Disease Modeling, Bellevue, WA, 98005

arXiv:1611.08056v2 [cs.SY] 2 Aug 2017

Kristi A. Morgansen, Mehran Mesbahi
University of Washington, William E. Boeing Department of Aeronautics and Astronautics, Seattle, WA
98195

Abstract
This paper is concerned with the design of optimal control for finite-dimensional controlaffine nonlinear dynamical systems. We introduce an optimal control problem that
specifically optimizes nonlinear observability in addition to ensuring stability of the
closed loop system. A recursive algorithm is then proposed to obtain an optimal
state feedback controller to maximize the resulting non-quadratic cost functional. The
main contribution of the paper is presenting a control synthesis procedure that provides closed loop asymptotic stability, on one hand, and empirical observability of the
system, as a transient performance criteria, on the other.
Keywords: Nonlinear observability, optimal state feedback, stability, gradient-based
algorithm

1. Introduction
A primary consideration in the control synthesis examined in this paper is system
observability. This is motivated by the inherent coupling in the actuation and sensing
in nonlinear systems; as such, one might be able to make a system more observable by
5

changing the control inputs [1, 2, 3].
Our goal in this work is designing a process for efficiently choosing the inputs of
a nonlinear system to improve the overall sensing performance (measured by the empirical Gramian). This task is motivated through problems where the reconstruction of

Preprint submitted to Systems & Control Letters

August 3, 2017

the state is challenging and it becomes imperative to actively acquire, sense, and esti10

mate it; an example of such a problem is robot localization operating in an unknown
environment. The problem of active sensing has received significant attention in the
robotics community in recent years. In this context, a robot can use the path that maximizes observability to more effectively localize itself or to effectively construct a map
of the environment [4, 5]. In [4], sampling trajectories for autonomous vehicles are

15

selected to maximize observability of an environmental flow field. In that work, an
iterative technique has been used on a finite sampling of parameters instead of examining all possible sampling trajectories. Moreover, the resulting optimized path was
computed by an exhaustive search. In [5], an optimal path is given for a particular case
of omnidirectional vehicle moving in a planar environment with only two markers. Yu

20

et al. [6] have developed a path planning algorithm based on dynamic programming
for navigation of a micro aerial vehicle (MAV) using bearing-only measurements. In
their proposed algorithm, at each time step, one of the three possible choices of roll
command is selected by solving an optimization problem based on an observability
criterion for the chosen path. Each of these approaches are applicable for a specific

25

type of nonlinear systems; solving the problem of choosing controls to improve the
observability of an arbitrary nonlinear system in general is a challenging problem.
The fundamental step toward designing an observability-based optimal controller is
to formulate the goal of control as a long-term optimization of a scalar cost function. In
order to formulate this control design problem using optimal control, one must define

30

an objective that be used to evaluate the long-term performance of running each candidate control policy, u(x) (from each initial condition x0 ), subject to relevant algebraic
and dynamic constraints. Several measures have been used in the literature to evaluate the observability of a system. In the case of the observability-based optimization
problem, the observability Gramian has been the most popular notion used to evaluate

35

observability. In order to perform an optimization on the observability Gramian matrix, a scalar function of the matrix must be chosen. The smallest singular value [7, 8],
the determinant [9, 10, 11], the trace [12, 13], and the spectral norm [14] of the observability Gramian are candidate measures for observability optimization. However,
the analytical computation of the nonlinear observability Gramian is expensive and re2

40

quires that the system dynamics be described by smooth functions. An alternative tool
used to measure the level of observability for nonlinear systems is the empirical observability Gramian. To compute the empirical observability Gramian, one only needs
to collect empirical data (either from experiments or simulation).
For nonlinear systems, optimal state feedback can be obtained from the solution of

45

the Hamilton-Jacobi-Bellman (HJB) equation. This equation is usually hard to solve
analytically. Over the last few years, with increasingly powerful computers and efficient algorithms, the popularity of numerical optimal control has considerably grown.
In this direction, different approximation methods have been used to obtain suboptimal
feedback control policies for quadratic [15, 16] or non-quadratic [17, 18] cost func-

50

tionals. Most of these solution methods involve solving Riccati equations and a series
of linear algebraic or differential equations. In this work, we use a recursive algorithm
to find a solution to our optimization problem that maximizes the system observability. Along the way, we approximate the solution to the optimal control with piecewise
linear controls.

55

In comparison with the previous observability-based path planning algorithms reported in the literature, the state feedback obtained in this paper can be applied to a
larger family of nonlinear systems; the algorithm also ensures the closed loop stability.
In particular, utilizing the concept of observability Gramian, and tools from optimal
control theory, this paper develops an algorithm for suboptimal piecewise linear state

60

feedback controller with stability guarantees. The objective function examined in this
paper is quadratic in state and control, augmented with an observability measure. One
of our contribution is presenting an observability index, whose optimization does not
have adverse effects on the stability of the system.

2. Mathematical Preliminaries
65

Consider a linear time invariant system of the form
ẋ = Ax + Bu
(1)
y = Cx ,

3

for which, the observability Gramian,
Z tf

Wo,L =

T

eA t CT CeAt dt ,

(2)

0

can be computed to evaluate the observability of a linear system [8]. A linear system is (fully) observable if and only if the corresponding observability Gramian is full
rank [19]. In the linear setting, if the observability Gramian is rank deficient, certain
states (or directions in the state space) cannot be reconstructed from the sensor data,
70

regardless of the control policy being applied. While the observability Gramian works
well for determining the observability of linear systems, analytical observability conditions for nonlinear systems quickly become intractable, necessitating modifications
for computational tractability. One approach is evaluation of nonlinear observability
Gramian through linearization and the corresponding Jacobian matrices; however, this

75

approach only provides an approximation of the local observability for a specific trajectory. One alternative method to evaluate observability of a nonlinear system is using the
relatively new concepts of the observability covariance or the empirical observability
Gramian [8]. This approach provides a more accurate description of a nonlinear system’s observability, while being less computationally expensive than analytical tools

80

such as Lie algebraic based methods. To set the stage for the main contribution of the
paper, let us first provide a brief overview of the empirical observability Gramian and
how it is used to evaluate the observability of nonlinear systems.
Consider the problem of system observability for nonlinear systems in control
affine form,

p


ẋ = f0 (x) + ∑ fi (x)ui , x ∈ Rn ,
i=1




y = h(x), y ∈ Rm .

(3)

For such systems, f0 (x) is referred to as the drift vector field or simply drift, and fi (x)
are termed the control vector fields.
The empirical observability Gramian for the nonlinear system (3) is constructed as
follows. For ε > 0, let x±i
0 = x0 ± εei be the perturbations of the initial condition and
y±i (t) be the corresponding output, where ei is the ith unit vector in Rn . For the system

4

(3), the empirical observability Gramian Wo , is an n × n matrix, whose (i, j) entry is
Woi j =
85

1
4ε 2

Z tf

T + j

y+i (t) − y−i (t)
y (t) − y− j (t) dt.

(4)

0

It can be shown that if the system dynamics (f0 and fi functions in (3)) are smooth,
then the empirical observability Gramian converges to the local observability Gramian
as ε → 0. Note that the perturbation, ε, should always be chosen such that the system
stays in the region of attraction of the equilibrium point of the system. The largest
singular value [14], the smallest eigenvalue [8], the determinant [4, 11], and the trace

90

of the inverse [4] of the observability Gramian have been used as different measures
for the observability.
The empirical observability Gramian (4) is used to introduce an index for measuring the degree of observability of a given nonlinear system. This index is called the
observability index; here we will use the trace of the empirical observability Gramian
for the measure of the observability, i.e.,
trace(W ) =

1
4ε 2

Z tf n
0

∑

h(x+i (t)) − h(x−i (t))

2

dt ,

(5)

i=1

where, x±i (t) is the trajectory corresponding to the initial condition x±i
0 = x0 ± εei .
3. Problem Formulation
In order to preserve the asymptotic convergence of the closed loop state trajectory,
95

we consider an augmented optimal control problem. In this augmented control problem, the control objective is twofold:
• Improve the observability of the system in order to avoid unobservable trajectories, and
• Guarantee the stability of the system and convergence of the resulting state tra-

100

jectory.
In this direction, we consider the following total cost functional
min J =
x,u

Z tf 
0

l1 (x, u) − l2 (t, x, x±1 , · · · , x±n ) dt ,
5

(6)

l1 (x, u) = xT Qx + uT Ru
±1

±n

1 n
∑ h(x+i ) − h(x−i )
4ε 2 i=1

−t

l2 (t, x, x , · · · , x ) = e satζ

(7)

!
2

,

where, satζ (x) is
satζ (x) =


 ζ if x > ζ


(8)

x if x ≤ ζ

and ζ > 0 is chosen such that the optimization problem (6) is feasible and meaningful.
The proposed cost functional consists of two parts:
• The first term, l1 (x, u), takes into account the control energy and the deviation
from the desired steady state trajectory; in our setup we assume that Q and R are
105

both positive definite.
• The observability index, l2 (t, x, x±1 , · · · , x±n ), which is a transient term, taking
into account the local observability of the system.
In the cost function (6), the sum of the control effort (i.e., the integral of the uT Ru
term) and the deviation from the desired trajectory (i.e., the integral of the xT Qx term)
is minimized, while maximizing an observability index. The index l1 (x, u) determines
the asymptotic behavior of the closed loop system, while l2 (t, x, x±1 , · · · , x±n ) specifies the desired transient behavior of the system. This cost function does not directly
maximize observability. Instead the observability term, l2 (t, x, x±1 , · · · , x±n ), tunes the
cost function so that the obtained optimal control makes the system more observable.
We note that maximizing the index of observability and ignoring the remaining terms
does not guarantee the stability of system. In order to have a meaningful optimization
problem, it is desired to have xT Qx − l2 (·) ≥ 0. The saturation limit ζ in (8), ensures
that xT Qx − l2 (·) remains a positive semi-definite function. However, the saturation
limit can be set to a constant number, for example, when we have information on the
rate of convergence of the system. Given a nonlinear system ẋ = f(x, u), assume that
there is a lower-bound, β , on the rate of decay of the nonlinear system, such that
kx(t)kQ ≥ e−βt kx(t j )kQ , ∀t ∈ [t j ,t j+1 ) ,
6

(9)

where k · kQ is the norm of the vector induced by the positive definite matrix Q. In this
case, the saturation parameter, ζ , could be set to

1

 kx(t j )k2Q
if 0 < β ≤ ,
2
ζ=
1

 e(1−2β )t j+1 kx(t j )k2 if
β> .
Q
2

(10)

Lemma 1. Consider the nonlinear system ẋ = f(x, u). If condition (9) is satisfied, then
the saturation parameter ζ , given by (10), guarantees that l0 (x) = xT Qx − l2 (·) ≥ 0.
Proof. From (10) we have
ζ ≤ e(1−2β )t kx(t j )k2Q , ∀t ∈ [t j ,t j+1 ) .

(11)

On the other hand, using (9), one can conclude that
ζ ≤ et kx(t)k2Q .

(12)

From the definition of the saturation function (8), we have satζ (·) ≤ ζ . Hence,
l2 (·) = e−t satζ (·) ≤ kx(t)k2Q ,
110

(13)

and xT Qx − l2 (·) ≥ 0, for all t ∈ [t j ,t j+1 ).
The problem considered here is a nonlinear optimal control with a non-quadratic
cost function (6). This problem is generally solved by solving a partial differential
equation (Hamilton-Jacobi-Bellman (HJB) equation). In general, HJB does not have
an analytical solution and potentially difficult to solve computationally. In order to
simplify the derivation of the control input, the original problem is divided into a sequence of subproblems of finding an optimal control in the state feedback form, namely
u = Kx, over shorter time intervals; as such, the resulting control input can be viewed
as a sub-optimal solution to the original problem. This approach motivates designing
a piece-wise linear state feedback for the augmented optimal control problem. Let
0 = t0 < t1 < · · · < t j < t j+1 < · · · < t f . Then the optimal linear state feedback control
u j for the time interval t ∈ [t j t j+1 ), j = 0, · · · , f − 1 is given by
u∗j =argmin
x,u

Z t j+1 
tj

l1 (x, u) − l2 (t, x, x±1 , · · · , x±n ) dt + L(x(t j+1 ),t j+1 )
(14)

subject to u = K j x, t ∈ [t j t j+1 ) ,
7

where, L(x(t j+1 ),t j+1 ) is the cost-to-go at the final time of the interval [t j t j+1 ). Without loss of generality, consider a time interval [t j t j+1 ). By the substitution of u = K j x
into (14), the cost functional can then be approximated as,
Z t j+1

J(K j ) =

tj

Γ(t, x, x±1 , · · · , x±n , K j )dt + L(x(t j+1 ),t j+1 ) ,

(15)

where

Γ(t, x, x±1 , · · · , x±n , K j ) = xT K Tj RK j + Q x − l2 (t, x, x±1 , · · · , x±n ) .

(16)

Now, considering the dynamics of the nonlinear system, the optimization problem can
be expressed as
Z t j+1

minimize
Kj

J(K j ) =

tj

Γ(t, x, x±1 , · · · , x±n , K j )dt + L(x(t j+1 ),t j+1 )
(17)

subject to ẋ = f(x, K j ), x(t j ) = x j ,
ẋ±k = f(x±k , K j ), x±k (t j ) = x j ± εek , k = 0, · · · , n .
The optimal gain K ∗j need to be found to minimize this cost-to-go J.
4. Optimization Solution

In this section, an algorithm is presented to solve (17) to determine the control policy for the optimal control objective that has an embedded observability index. Here,
115

a recursive gradient-based algorithm is devised to find a solution to this optimization
problem.
Given (15), we first define a new variable, xn+1 ,
Z t

xn+1 (t) =

tj

Γ(τ, x(τ), x±1 (τ), · · · , x±n (τ), K j )dτ + L(x(t),t).

(18)

Assuming x(t j ) = x j is given, it is clear that xn+1 (t j ) = L(x j ,t j ) and xn+1 (t j+1 ) = J(K j ).
Next, define an augmented state vector as


x(t)





 ±1 
 x (t) 


 . 
x̄(t) =  ..  .


 ±n 
 x (t) 


xn+1 (t)
8

Then,


ẋ





xj








 ±1 

x j ± εe1 
 ẋ 




..


 . 
x̄˙ =  ..  = H(t, x̄, K j ) ; x̄(t j ) = 
,
.






 ±n 
x j ± εen 
 ẋ 




ẋn+1
L(x j ,t j )

(19)

where,


f(x, K j )







f(x±1 , K j )




..


H(t, x̄, K j ) = 
,
.




±n , K )


f(x
j


±1
±n
Γ(t, x, x , · · · , x , K j ) + L̇(x,t)

(20)

and f(x, K j ) is given in (3) after substituting u = K j x. The optimization problem can
now be formulated as:
minimize
Kj

J = xn+1 (t j+1 )
(21)

subject to x̄˙ = H(t, x̄, K j ).
∂J
∂ xn+1
=
,
∂ Kj
∂ K j t=t j+1
is required. In [18], a method is demonstrated for solving a similar problem setup. By
∂ x̄
defining X̄K j =
, and applying the chain rule, we have
∂ Kj
In order to solve this optimization problem, the gradient vector,

∂H
∂H
∂H
∂ H ∂ x̄
+
=
X̄K j +
,
X̄˙K j =
∂ x̄ ∂ K j ∂ K j
∂ x̄
∂ Kj
X̄K j (t j ) = 0.
120

(22)

∂ x̄
is the
∂ Kj
derivative of a vector with respect to a matrix, which results in a higher order tensor.

Notice that if the system is multi-input, then K j is a matrix and the term

In order to address multi-input as well as single-input control problems using more
∂ x̄
conventional linear algebra, we need to have separate equations for X̄ki j =
,i =
∂ ki j
1 . . . p, where ki j is the ith row of the matrix K j .
125

Now (19) and (22) should be solved together for t j < t < t j+1 . The last row of X̄K j at
t = t j+1 is the gradient vector which can be used for improving the optimal value of K j .
9

Based on these observations, an iterative algorithm for obtaining the optimal feedback
gain, K ∗j , is presented in Algorithm 1. Note that we are using an estimate of the Hessian
matrix to make sure that the optimal point is a minima of the objective (and not a saddle
130

point). One can use a more precise method (e.g. a finite difference approximation for
estimating the Jacobian presented in [20]). Since we are not directly using the Hessian
in the computation of the optimal point, we are using a simple method to estimate the
curvature of the function at each iterative step.
Choose step size µ0 > 0;
Choose K 0j , an initial value for K j that stabilizes the system;
cvxCheck : = TRUE;
Choose a convergence tolerance εt ;
repeat
Solve (19) and (22) together for t j < t < t j+1 ;
gi := the last row of X̄K j at t = t j+1 ;
= K ij − µi gi ;
K i+1
j
Calculate H such that H[mn] =

gi [m]−gi−1 [m]
;
i
K i+1
j [n]−K j [n]

if H  0 then
cvxCheck := TRUE ;
else
cvxCheck := FALSE ;
end
if cvxCheck is TRUE then
Update step size µi ;
else
Do not change step size;
end
until kgi k ≤ εt AND cvxCheck is TRUE;
Algorithm 1: Iterative algorithm to find K ∗j
The iteration terminates when the gradient vector becomes small enough or remains

10

135

almost unchanged. The algorithm also checks the convexity of the objective at the
solution obtained in order to ensure that it corresponds to a local minimum. The value
of K j is updated based upon the gradient descent rule. The step size µi is a positive
number, chosen small enough such that system stability is guaranteed.
4.1. Convergence of the Algorithm
The gradient method is a popular first order method due to its simplicity. The

140

algorithm requires the computation of the gradient, but not the second derivatives. One
of the parameters that must be chosen in this method is the step size.
Proposition 1. Any direction that makes an angle of strictly less than

π
2

with −∇J is

guaranteed to produce a decrease in J provided that the step size is sufficiently small
145

[21].
The suggested rule for the step size selection here is
µi =

µ0
,
i

(23)

where, 0 < µ0 < ∞ is constant, and i is the iteration counter. This step size policy,
which is called square summable but not summable, satisfies
∞

∞

∞
µ0
1
= µ0 ∑ = ∞
i
i=1
i=1 i

∑ µi = ∑

i=1
∞

∞
µ02
1
= µ02 ∑ 2 < ∞.
2
i
i
i=1
i=1
∞

(24)

∑ µi2 = ∑

i=1

It is worth noting that because of the transient term, l2 (·), the cost functional J(K)
is not convex. But since the transient term is bounded, the cost functional is bounded
by two convex functions. The lower and upper bounds for the cost functional is given
by
Z t j+1

J=
J¯ =

tj
Z t j+1
tj

{l1 (x, u) − ζ } dt + L(x(t j+1 ),t j+1 ) ,
(25)
l1 (x, u)dt + L(x(t j+1 ),t j+1 ) .

¯ are convex functions. Therefore, the cost
Both of the lower and upper bounds (J and J)
function cannot be concave over the entire search space and there should exist regions

11

over which the cost function is locally convex. We assume here that after a finite
number of iterations, the algorithm drives the system to a locally convex point and
150

after that, it remains in a convex region around that point until the algorithm converges
to a local minimum.
Theorem 1. If J(K) is Lipschitz continuous, then Algorithm 1 converges to K ∗ , where
K ∗ is a local minimum of J.
Proof. This proof is similar to the convergence proof for the gradient descent algorithm
for a convex function [22]. Since J(K) satisfies the Lipschitz condition, the norm of
the gradient vectors, gi , are bounded, i.e., there is a G such that kgi k2 ≤ G for all i, and

|J(Ki1 ) − J(Ki2 )| ≤ GkKi1 − Ki2 k2 ,

(26)

for all i1 and i2 . Since K ∗ is a local minimum of J, so there is a neighborhood U
containing K ∗ such that the function is convex on this neighborhood. It is assumed
here that K j ∈ U, j = 0, ..., i + 1, so we have
kK i+1 − K ∗ k22 = kK i − µi gi − K ∗ k22 = kK i − K ∗ k22 − 2µi gi (K i − K ∗ ) + µi2 kgi k22

≤ kK i − K ∗ k22 − 2µi J(K i ) − J ∗ + µi2 kgi k22 ,
(27)
where, J ∗ = J(K ∗ ). The inequality above comes from the property of the gradient
vector of a convex function which is
J(K ∗ ) ≥ J(K i ) + gi (K ∗ − K i ).

(28)

Applying the inequality above recursively, we have
i
i

kK i+1 − K ∗ k22 ≤ kK 0 − K ∗ k22 − 2 ∑ µ j J(K j ) − J ∗ + ∑ µ 2j kg j k22 ,

(29)

i
i

2 ∑ µ j J(K j ) − J ∗ ≤ kK 0 − K ∗ k22 + ∑ µ 2j kg j k22 .

(30)

j=0

j=0

then
j=0

j=0

(i)

If we define Jsol = min J(K j ) as the solution of the algorithm after i iterations, then
j=0,...,i

we have
(i)

Jsol − J ∗ ≤

kK 0 − K ∗ k22 + ∑ij=0 µ 2j kg j k22
2 ∑ij=0 µ j
12

.

(31)

Finally, using the assumption kgi k2 ≤ G, we obtain the inequality
(i)

Jsol − J ∗ ≤

kK 0 − K ∗ k22 + G2 ∑ij=0 µ 2j
2 ∑ij=0 µ j

.

(32)

If K i ∈ U as i → ∞, then by applying the conditions given in (24)
(i)

lim Jsol − J ∗ ≤ lim

i→∞

kK 0 − K ∗ k22 + G2 ∑ij=0 µ 2j
2 ∑ij=0 µ j

i→∞

= 0.

(33)

Therefore, the gradient method proposed above converges to the local minimum K ∗ .
155

Otherwise, if at an iteration, say k < ∞, K k ∈
/ U, then there is always a finite number
0

of iterations, say k0 , such that K k+k ∈ U 0 , where U 0 is also a region over which the
function is convex, and it contains a local minimum. We can use the same proof given
above for this local minimum. Hence, the algorithm converges to a local minimum.
The discussion above ensures that the proposed algorithm is convergent. Other
160

methods, such as Newton’s method, can be used to improve the rate of convergence.
But these methods require the computation of the Hessian matrix ∇2 J or its approximation. Explicit computation of the Hessian matrix is often an expensive process. In
this direction, approaches such as the quasi-Newton method can be used to estimate the
Hessian matrix. Any of these higher order methods can be used to achieve higher rate

165

of convergence at the cost of increased computational complexity. Improving the rate
of convergence of first order methods is not the main focus in this paper; the reader is
referred to [21] for such discussions.
5. Closed Loop Stability
In this section, the stability of the system in the sense of Lyapunov is investigated.

170

The idea of consecutive design of stabilizing control by Lyapunov function is used to
prove the stability of the system; this approach is inspired by LQR-Trees presented in
[23].
Theorem 2. Given the cost function (15), if the transient term is upper bounded by an
integrable function b(t):
l2 (t, x) ≤ b(t), ∀t ≥ 0, x ∈ Rn , u ∈ R p
L (t) =

Z ∞

(34)
b(τ)dτ < ∞, ∀t ≥ 0 ,

t

13

and the terminal cost function is positive semi-definite, continuously differentiable, and
decreases faster than the value of the function l1 (x, u) along all admissible control
175

inputs, u, and the corresponding trajectories, x, then the optimal control u = K ∗ x will
cause the trajectory of the closed loop system to converge to the origin.
Proof. The admissible function for l2 (·), introduced in (7), satisfies the conditions
given in (34). The condition on the terminal cost, L(t, x) is given as:
L̇(t, x) = Lt + Lx f(x, u) ≤ −l1 (x, u).

(35)

Suppose u∗ and x∗ are the optimal control and its corresponding trajectory obtained
from optimization problem (14) for t ∈ [t j t j+1 ). To show the stability of this control,
a Lyapunov function is defined as:
Z t+∆t

V (t) =
t

{l1 (x∗ , u∗ ) − l2 (t, x∗ ) + b(t)} dt +L (t +∆t)+L(x(t +∆t),t +∆t) . (36)

Note that all terms in (36) are positive, therefore, V (t) > 0. Now we need to show
that the Lyapunov function (36) is decreasing. In fact, the inequality V (t + δ ) < V (t)
should hold for any δ > 0, such that t + δ ∈ [t j t j+1 − ∆t]. At time t + δ :
Z t+δ +∆t

V (t + δ ) =
t+δ

{l1 (x∗ , u∗ ) − l2 (t, x∗ ) + b(t)} dt + L (t + δ + ∆t)

+L(x(t + δ + ∆t),t + δ + ∆t)
=V (t) −
Z

Z t+δ

t
t+δ +∆t

+
t+∆t

{l1 (x∗ , u∗ ) − l2 (t, x∗ ) + b(t)} dt +

Z t+δ +∆t
t+∆t

{l1 (x∗ , u∗ )} dt

{−l2 (t, x∗ ) + b(t)} dt + L (t + δ + ∆t) − L (t + ∆t)

+L(x(t + δ + ∆t),t + δ + ∆t) − L(x(t + ∆t),t + ∆t).
(37)
Based on the assumption (34), we have
Z t+δ +∆t
t+∆t

{−l2 (t, x∗ ) + b(t)} dt ≤

Z t+δ +∆t
t+∆t

b(t)dt = L (t + ∆t) − L (t + δ + ∆t), (38)

and from assumption (35), one can conclude
−

Z t+δ +∆t
t+∆t

l1 (x∗ , u∗ )dt ≥

Z t+δ +∆t

L̇dt = L(x(t +δ +∆t),t +δ +∆t)−L(x(t +∆t),t +∆t).
t+∆t

(39)
14

Therefore,
V (t + δ ) ≤ V (t) −

Z t+δ
t

{l1 (x∗ , u∗ ) − l2 (t, x∗ ) + b(t)} dt < V (t).

(40)

This proof guarantees the convergence of the closed loop state trajectory x(t) to the
origin. The stability in the sense of Lyapunov can be proved for all intervals [t j t j+1 ).
The basin of attraction obtained from Lyapunov function in [t j t j+1 ) contains the goal
180

state of the previous interval, x(t j ). Therefore, the optimal control obtained here will
stabilize the closed loop system.
Note that the effect of the terminal cost on the stability of the closed loop system has
been studied for linear systems as well as nonlinear systems [24]. By replacing l1 with
l1 + l2 in (35), we obtain the well known condition on the final cost for convergence to

185

the origin of the finite-horizon optimal control problem; this step of our analysis has
been inspired by the work of Alessandretti et al. [25].
6. An Illustrative Example
The control synthesis procedure presented in this paper is now illustrated on a holonomic system with a nonlinear measurement. The dynamical system is given by
ẋ1 = u1 ,

(41)

ẋ2 = u2 .
In this case, we have a linear system ẋ = Ax +Bu, where, A = 0 and B = I. By choosing
Q = R = I, and solving the algebraic Riccati equation AT P + PA − PBR−1 BT P + Q = 0,
we obtain P = I. Thus, the control policy uLQR = −R−1 BT Px = −x asymptotically
stabilizes (41). Utilizing this state feedback controller requires full state estimation,
which results in having an observable system. If any portion of the trajectory is not
observable, we have no means of building the feedback controller. Due to this coupling
between sensing and control in nonlinear systems, we can investigate the observability
of the system after applying the controller. Assume that the position of the vehicle is
continuously measured by an omni-directional camera centered at the origin. Then, the
output function is given as
y=

x2
,
x1

15

(42)

where y ∈ R is a bearing only measurement, providing information on the direction of
the vehicle but not on the distance. The observability matrix is given by

  
1
− xx22
y
x
1

1
 

∂ 
  
x2
1
1

dO =
ẏ = 2 x3 u1 − x2 u2 − x2 u1 
.
∂x   
1
1
1

..
..
.. 
.
.
.

(43)

The system is locally observable if the observability matrix dO, is full rank [26]. The
observability matrix for nonlinear system has infinite rows. Here, we can show that
there exists a set of control inputs such that the observability matrix becomes full rank.
Let us consider the first two rows of the observability matrix; then we have


1
x2
det(dO) = 3 u2 − u1 .
x1
x1
Therefore, for all controls u2 6=

x2
x1 u1

the observability matrix is full rank, and as a

result, the system is observable. Now, if we substitute the optimal control obtained
from LQR, uLQR = −x, we have
ẏ =

−x1 x2 + x1 x2
ẋ2 x1 − ẋ1 x2
=
= 0.
2
x1
x12

(44)

All other differential terms are zero (ÿ = y(3) = . . . = 0). Hence by applying the control
uLQR = −x, the observability matrix is not full rank, and the system is not observable
190

with this choice of control. Therefore, while we have optimal controls for this type of
linear system, the nonlinear measurement needs to be managed appropriately.
Now add the observability index to the optimization problem. Assume that the
instantaneous cost is given by:
l(t) = x(t)T x(t) + u(t)T u(t) − e−t satζ

2
1 2 +i
∑ y (t) − y−i (t)
4ε 2 i=1

!
,

(45)

and assume t f = 100. The final cost needs to be chosen to meet the condition (35).
The final cost can be set to L(x f ) = xTf Q f x f , where Q f = 0.1I. The time span [t0 ,t f ]
is divided into segments [0, 1), [1, 2), · · · , [ti ,ti+1 ), · · · , [99, 100). Thus, we have 100
intervals, and for each interval the optimal K ∗j for t ∈ [t j t j+1 ) is given by:
K ∗j = argmin
Kj

Z t j+1 
tj

l1 (x, K j ) − l2 (t, x, x±1 , x±2 ) dt + x(t j+1 )T Q f x(t j+1 ) .

16

(46)

Here, we compare the optimal control obtained from the LQR cost function without
the observability term and the cost function given in (46) which considers the observ-

195

ability of the system. The
trajectories obtained from these two scenarios
 resulting

−1
for the initial condition   are depicted in Fig. 1. The optimal controls for these
2
two scenarios are given in Fig. 2. As it can be seen in Fig. 2, initially the absolute
values of the observability-based controls are smaller than their corresponding LQR
optimal controls. This comes from the fact that by adding the observability term to
the optimization, we in fact decrease the cost of being away from the origin. There-

200

fore, the controller chooses a trajectory that is not necessarily the closest path to the
origin. However, over time, the observability term becomes less dominant, and the
absolute values of the observability-based control take the lead with respect to their
corresponding LQR optimal control to satisfy the asymptotic stability condition. The
observability-based optimal control initially keeps the system away from an unobservable trajectory, while keeping the shortest path to the origin, (as shown in Fig. 1),
and eventually returns to the desired equilibrium. Although, the observability-based
controller chooses the longer path, it guarantees the system observability in order to
estimate the state information required for the state feedback control.
Optimal trajectories

2

adding observability term
LQR

1.8
1.6
1.4
1.2

x2

205

1
0.8
0.6
0.4
0.2
0
-1.5

-1

-0.5

0

0.5

x1
Figure 1: Trajectories for the system (41) and measurement (42) using LQR optimal state feedback control
without (solid, dark color) and with (dashed, light color) observability-based optimal control.

17

0.6
0.4
0.2
0

u

-0.2
-0.4
u 1 - with observability term

-0.6

u 2 - with observability term
u 1 - only LQR term

-0.8

u 2 - only LQR term

-1
-1.2
0

5

10

15

20

25

30

t
Figure 2: Optimal controls for the system (41) and measurement (42) using LQR optimal state feedback
control without (dark color) and with (light color) observability-based optimal control.

7. Conclusion
210

This paper has been concerned with obtaining a suboptimal control for a nonlinear system based on a nonlinear observability criteria. We proposed an algorithm to
optimize a finite-horizon cost function that contains a term that determines asymptotic
behavior of the system in addition to a transient term responsible for maximizing a notion of nonlinear observability. In this direction, the empirical observability Gramian

215

has been used for improving the local observability for nonlinear systems. In addition,
stability of the system has been investigated; in particular, it was shown that under some
conditions on the terminal cost, the stability of the closed loop system is guaranteed.
The cost function was a combination of quadratic and non-quadratic terms and motivated by the desire to maximize the observability of the nonlinear system. Similar

220

to other global optimization problems, the main challenge in the proposed approach
is the difficulty in computing the global minimum. In order to work around this issue, a gradient-based recursive algorithm was used to obtain a piecewise optimal linear
state feedback controller. Linear state feedback is not necessarily optimal control for
18

general nonlinear systems. Furthermore, not all classes of nonlinear systems (e.g., non225

holonomic systems) can be stabilized by smooth feedback control [27, 28] or by time
invariant state feedback control. However, such systems can generally be stabilized by
time varying oscillatory control, and the next step in our work is considering appropriate time varying oscillatory feedback control for stabilization while improving the
observability of the system.

230

ACKNOWLEDGMENTS
The authors thank Bill and Melinda Gates for their active support and their sponsorship through the Global Good Fund.

References
References
235

[1] A. Alaeddini, K. A. Morgansen, Autonomous state estimation using optic flow
sensing, in: American Control Conference, 2013, pp. 585–590.
[2] B. T. Hinson, K. A. Morgansen, Observability optimization for the nonholonomic
integrator, in: American Control Conference, 2013, pp. 4257–4262.
[3] B. T. Hinson, M. K. Binder, K. A. Morgansen, Path planning to optimize observ-

240

ability in a planar uniform flow field, in: American Control Conference, 2013,
pp. 1392–1399.
[4] L. DeVries, S. J. Majumdar, D. A. Paley, Observability-based optimization of
coordinated sampling trajectories for recursive estimation of a strong, spatially
varying flowfield, Journal of Intelligent & Robotic Systems 70 (1-4) (2013) 527–

245

544.
[5] F. Lorussi, A. Marigo, A. Bicchi, Optimal exploratory paths for a mobile rover, in:
International Conference on Robotics and Automation, Vol. 2, 2001, pp. 2078–
2083.

19

[6] H. Yu, R. Sharma, R. W. Beard, C. N. Taylor, Observability-based local path
250

planning and collision avoidance for micro air vehicles using bearing-only measurements, in: American Control Conference, 2011, pp. 4649–4654.
[7] W. Waldraff, D. Dochain, S. Bourrel, A. Magnus, On the use of observability
measures for sensor location in tubular reactor, Journal of Process Control 8 (5-6)
(1998) 497–505.

255

[8] A. J. Krener, K. Ide, Measures of unobservability, in: Proceedings of the 48th
Conference on Decision and Control, held jointly with the 2009 28th Chinese
Control Conference, 2009, pp. 6401–6406.
[9] A. Alaeddini, K. A. Morgansen, Optimal disease outbreak detection in a community using network observability, in: American Control Conference (ACC), IEEE,

260

2016, pp. 7352–7357.
[10] A. V. Wouwer, N. Point, S. Porteman, M. Remy, An approach to the selection
of optimal sensor locations in distributed parameter systems, Journal of process
control 10 (4) (2000) 291–300.
[11] M. Serpas, G. Hackebeil, C. Laird, J. Hahn, Sensor location for nonlinear dy-

265

namic systems via observability analysis and max-det optimization, Computers
& Chemical Engineering 48 (2013) 105–112.
[12] A. Alaeddini, K. Morgansen, M. Mesbahi, Adaptive communication networks
with privacy guarantees, in: American Control Conference (ACC), 2017, pp.
4460–4465. doi:10.23919/ACC.2017.7963642.

270

[13] F. Van den Berg, H. Hoefsloot, H. Boelens, A. Smilde, Selection of optimal sensor
position in a tubular reactor using robust degree of observability criteria, Chemical engineering science 55 (4) (2000) 827–837.
[14] A. K. Singh, J. Hahn, Determining optimal sensor locations for state and parameter estimation for stable nonlinear systems, Industrial & Engineering Chemistry

275

Research 44 (15) (2005) 5645–5659.

20

[15] S. Beeler, H. Tran, H. Banks, Feedback control methodologies for nonlinear systems, Journal of Optimization Theory and Applications 107 (1) (2000) 1–33.
doi:10.1023/A:1004607114958.
[16] Y. Chen, T. Edgar, V. Manousiouthakis, On infinite-time nonlinear quadratic op280

timal control, in: Proceedings of the 42nd IEEE Conference on Decision and
Control, Vol. 1, 2003, pp. 221–226 Vol.1.
[17] T. Çimen, S. P. Banks, Global optimal feedback control for general nonlinear
systems with nonquadratic performance criteria, Systems & Control Letters 53 (5)
(2004) 327–346.

285

[18] P. Esfahani, F. Farokhi, M. Karimi-Ghartemani, Optimal state-feedback design
for non-linear feedback-linearisable systems, Control Theory & Applications,
IET 5 (2) (2011) 323–333.
[19] P. Muller, H. Weber, Analysis and optimization of certain qualities of controllability and observability for linear dynamical systems, Automatica 8 (3) (1972)

290

237–246.
[20] J. C. Spall, Adaptive stochastic approximation by the simultaneous perturbation
method, IEEE transactions on automatic control 45 (10) (2000) 1839–1853.
[21] S. Wright, J. Nocedal, Numerical optimization, Vol. 2, Springer New York, 1999.
[22] S. Boyd, L. Xiao, A. Mutapcic, Subgradient methods, lecture notes of EE392o,

295

Stanford University, Autumn Quarter 2004.
[23] R. Tedrake, I. R. Manchester, M. Tobenkin, J. W. Roberts, Lqr-trees: Feedback
motion planning via sums-of-squares verification, The International Journal of
Robotics Research 29 (8) (2010) 1038–1052.
[24] F. A. Fontes, A general framework to design stabilizing nonlinear model predic-

300

tive controllers, Systems & Control Letters 42 (2) (2001) 127–143.

21

[25] A. Alessandretti, A. P. Aguiar, C. N. Jones, A model predictive control scheme
with additional performance index for transient behavior, in: Proceedings of the
52nd Conference on Decision and Control, 2013, pp. 5090–5095.
[26] H. Nijmeijer, A. Van der Schaft, Nonlinear dynamical control systems, Springer
305

Science & Business Media, 1990.
[27] W. Lin, Feedback stabilization of general nonlinear control system: A passive
system approach, Systems & Control Letters 25 (1995) 41–52.
[28] V. N. Phat, Global stabilization for linear continuous time-varying systems, Applied Mathematics and Computation 175 (2) (2006) 1730–1743.

22

