For the derivation of the minimum energy filter we require some basic re- sults from optimal control theory such as the famous Pontryagin Minimum Principle and the Hamilton-Jacobi-Bellman equation. We begin with some essential definitions and continue with the statement of the optimal control theory and the corresponding concepts that are required for solving it. The following definitions and Theorems from optimal control theory are well- known and can be found in Athans and Falb [5] as well as Fleming and Rishel [34] which provide an introduction to optimal control theory. In the sections belows we will follow the approach of Evans [32, chapter 10] in which a rigorous introduction to the field of optimal control is given.
2.3.1
Fixed-Time Optimal Control Problem
Let us consider the following dynamical system that can be understood as a differential equation of the unknown state x. δ(t) denotes the control variable. ˙x(t) =f (x(t), δ(t)) (2.7) x(t0) =x0. (2.8)
Here, x0 ∈ Rnis a given initial point and f : Rn×D → Rnis a given bounded
and Lipschitz-continuous function. D is a compact subset of Rm for a m∈ N.
In the following the variable δ = δ(t) is a control variable and the set of all measurable controls is the set of admissible controls, denoted by
D := {δ : [t0, t]→ D|δ is measurable } .
The objective of optimal control theory is to find an admissible control δ such that a certain optimality criterion is fulfilled and such that the control
2.3. Control Theory: A Brief Introduction variable sufficies (2.7). The optimality criterion is usually given by a cost function on the control variable δ that needs to be minimized. For each ad- missible control δ(·) ∈ D we define the corresponding cost or energy function
Jx,t0(δ) :=
Z t
t0
r(x(τ ), δ(τ )) dτ + g(x(t)) , (2.9) where x(·) = xδ(·)(·) solves the ordinary differential equation (ODE) (2.7).
The functions r : Rn × D → R and g : Rn → R are given cost functions
for the running costs and terminal costs, respectively. Following Evans [32] these functions are assumed to be bounded and Lipschitz-continuous. The goal of optimal control theory is to determine the value function that is defined through
V(x, τ) := inf
δ∈DJx,τ(δ), x∈ R
n, 0≤ τ ≤ t, (2.10)
such that the state equation (2.7) is fulfilled. It can be shown that the value functionV satisfies a specific kind of Hamilton-Jacobi equation which can be used to find an optimal control. This procedure is also known as Pontryagin Minimum Principle. This is a result of the following Theorem.
Theorem 2.3.1. The value function V is the unique viscosity solution of this terminal-value problem for the Hamilton-Jacobi-Bellman equations:
∂
∂τV(x, τ)+ mind∈Dhf(x, d), DxV(x, τ)i + r(x(τ), d) = 0 , (2.11)
V(·, t) =g(·) . (2.12)
Proof. See [32, chapter 10, Theorem 2].
Remark 2.3.2. Below we will use the term “pre-Hamiltonian” which is defined by
˜
H(x, p, δ, t) := hf(x(t), δ(t)), p(t)i + r(x(t), δ(t)) . (2.13) The Hamiltonian can then be defined as the minimum of the pre-Hamiltonian regarding δ, i.e. H(x, p, t) := min δ∈D hf(x(t), δ(t)), p(t)i + r(x(t), δ(t)) = min δ∈D ˜ H(x, p, δ, t) (2.14)
With the definition of the Hessian, the Hamilton-Jacobi-Bellman equation in (2.11) can also be written as
∂
∂tV(x, t) + H(x, DxV(x, t), t) = 0 . (2.15) Below we will introduce the main concepts of minimum energy filter. Un- fortunately, these require a different energy function than the function in- troduced in (2.9), where a condition on the terminal value is given by the function g. For the minimum energy filter, however, we need a condition on the initial value. The corresponding energy is
Jx,t(δ) :=
Z t
t0
r(x(τ ), δ(τ )) dτ + g(x(t0)), (2.16)
where x(·) = xδ(·)(·) solves the ordinary differential equation (ODE) (2.7).
By switching the integration bounds we obtain an expression, which is similar to the original one:
Jx,t(δ) :=−
Z t0
t
r(x(τ ), δ(τ )) dτ + g(x(t0)). (2.17)
The corresponding optimal control problem can be considered to run back- ward in timesuch that the pre-Hamiltonian needs to be defined as
˜
H(x, p, δ, τ) := hf(x(τ), δ(τ)), p(τ)i − r(x(τ), δ(τ)) . (2.18) Using the Pontryagin’s minimum principle we find the Hamiltonian for the control problem (2.17) is given as in (2.14) by
H(x, p, r) := min δ∈D hf(x(τ), δ(τ)), p(τ)i − r(x(τ), δ(τ)) = min δ∈D ˜ H(x, p, δ, τ) . (2.19) Theorem 2.3.1 states the Hamilton-Jacobi-Bellman equation for control prob- lems that are solved forward in time. However, we require a similar statement for control problems that are solved backwards in time. This state can be derived in a similar way as in Theorem 2.3.1 but the proof is involved. The most important part is, to replace the terminal value problem by the initial value problem. The corresponding value functionV is defined through
V(x, t) = inf
δ∈D
Z t
t0
2.3. Control Theory: A Brief Introduction As in Evans [32] one can derive a similar Hamilton-Jacobi-Bellman equation as in Theorem 2.3.1 for the value function (2.20) which reads
∂
∂tV(x, t) − H(x, DxV(x, t), t) = 0 . (2.21) For brevity we omit the proof of this equation which is beyond the scope of this work.
2.3.2
Observability and Controllability
Definition 2.3.3 (Observability). We say that a state x0 is observable at t0
if, given any control δ, there is a time t1 > t0 such that knowledge δ(τ ) and
the output y(τ ) for τ ∈ (t0, t1] is sufficient to determine x0. If every state x0
is observable at every time t0 in the interval of definition of the system, then
we say that the system is observable.
This definition means that the state variable can be uniquely determined from the observations y = y(t) and the knowledge about the trajectories of the noise processes δ = δ(t). When the function f is not injective, e.g. f (x) = sin(x) one can reconstruct the value of x only modulo 2π which is not unique. We provide another counter-example where consider the observation of the position of the epipole (focus of expansion) y(t) ∈ R2 which is not
sufficient to uniquely reconstruct the underlying camera motion.
Example 2.3.4. Let us consider the following filtering problem in which we want to determine the ego-motion of the camera E(t) = (R(t), w(t)) ∈ SE3,
where R(t)∈ SO3 is a rotation matrix and w(t)∈ R3 is a translation vector.
We assume that only observations of the epipole (focus of expansion) y(t)∈ R2 are given and that E is constant. This can be expressed as the following system:
˙
E(t) =E(t)δ(t) , E(t0) = E0, (2.22)
y(t) =π(R(t)>w(t)) , (2.23) where π : R3 → R2, (x
1, x2, x3)> 7→ x−13 (x1, x2)> denotes the perspective
projection onto the image plane. δ is a noise process which evolves on the tangent space of SE3 at identity, such that Eδ ∈ TESE3. Since the Lie
group SE3 is a six dimensional manifold, but the observations are only two
dimensional, there are four degrees of freedom for the camera motion that cannot be determined from epipole observations uniquely. Indeed the scale of the scene (magnitude of translation) get lost due to the projection or the rotation of the camera around the z-axis that leaves the epipole invariant.
Furthermore, changes in the translational and rotational components can be compensated by each other without changing the location of the epipole. Therefore, the underlying motion cannot be reconstructed uniquely and the control problem is not observable.
Definition 2.3.5 (Controllability). If the state x1 = 0 is reachable from x0
at t0, then we say that x0 is controllable at time t0. In other words, x0 is
controllable at t0 if there exists a piecewise continuous (control) function δ
such that
Φ(T ; δ(t0,t]), x0) = 0 , (2.24)
for some T ≥ t0, where Φ denotes the transition function of the system. If
every state x0 is controllable at any time t0 in the interval of definition of the
system, then we say that the system is (completely) controllable.