CV-STEM Estimation

Chapter 4: Convex Optimality in Robust Nonlinear Control and Estimation

4.2 CV-STEM Estimation

We could also design an optimal state estimator analogously to the CV-STEM control of Theorem 4.2, due to the differential nature of contraction theory that enables LTV systems-type approaches to stability analysis. In particular, we exploit the estimation and control duality in differential dynamics similar to that of the Kalman filter and LQR in LTV systems.

Let us consider the following smooth nonlinear systems with a measurement𝑦(𝑡), perturbed by deterministic disturbances𝑑_𝑒0(𝑥 , 𝑡)and𝑑_𝑒1(𝑥 , 𝑡)with sup𝑥 ,𝑡∥𝑑_𝑒0(𝑥 , 𝑡) ∥=

¯

𝑑_𝑒0 ∈ R^≥0and sup𝑥 ,𝑡∥𝑑_𝑒1(𝑥 , 𝑡) ∥ = 𝑑¯_𝑒1 ∈ R^≥0, or by Gaussian white noise, driven

by Wiener processes 𝒲₀(𝑡) and 𝒲₁(𝑡) with sup𝑥 ,𝑡∥𝐺_𝑒0(𝑥 , 𝑡) ∥𝐹 = 𝑔¯_𝑒0 ∈ R^≥0 and sup𝑥 ,𝑡∥𝐺_𝑒1(𝑥 , 𝑡) ∥𝐹 =𝑔¯_𝑒1 ∈R≥0:

¤

𝑥 = 𝑓(𝑥 , 𝑡) +𝑑_𝑒0(𝑥 , 𝑡), 𝑦 =ℎ(𝑥 , 𝑡) +𝑑_𝑒1(𝑥 , 𝑡) (4.3) 𝑑𝑥 = 𝑓(𝑥 , 𝑡)𝑑 𝑡+𝐺_𝑒0𝑑𝒲₀, 𝑦 𝑑 𝑡 =ℎ(𝑥 , 𝑡)𝑑 𝑡+𝐺_𝑒1𝑑𝒲₁ (4.4) where𝑡 ∈R^≥0is time,𝑥 :R^≥0↦→ R^𝑛is the system state,𝑦 :R^≥0↦→R^𝑚is the system measurement, 𝑓 : R^𝑛×R^≥0 ↦→ R^𝑛 and ℎ : R^𝑛 ×R^≥0 ↦→ R^𝑚 are known smooth functions,𝑑_𝑒0:R^𝑛×R≥0↦→ R^𝑛,𝑑_𝑒1 :R^𝑛×R≥0 ↦→R⁰,𝐺_𝑒0:R^𝑛×R≥0↦→R^𝑛×𝑤0, and 𝐺_𝑒1:R^𝑛×R≥0↦→ R^𝑛×𝑤1 are unknown bounded functions for external disturbances, 𝒲₀ :R≥0↦→R^𝑤0 and𝒲₁:R≥0↦→ R^𝑤1 are two independent Wiener processes, and the arguments of𝐺_𝑒0(𝑥 , 𝑡)and𝐺_𝑒1(𝑥 , 𝑡)are suppressed for notational convenience.

Let 𝐴(𝜚_𝑎, 𝑥 ,𝑥 , 𝑡ˆ )and𝐶(𝜚_𝑐, 𝑥 ,𝑥 , 𝑡ˆ ) be the SDC matrices given by Lemma 3.1 with (𝑓 , 𝑠,𝑠,¯ 𝑢¯) replaced by(𝑓 ,𝑥 , 𝑥 ,ˆ 0)and (ℎ,𝑥 , 𝑥 ,ˆ 0), respectively, i.e.

𝐴(𝜚_𝑎, 𝑥 ,𝑥 , 𝑡ˆ ) (𝑥ˆ−𝑥) = 𝑓(𝑥 , 𝑡ˆ ) − 𝑓(𝑥 , 𝑡) (4.5) 𝐶(𝜚_𝑐, 𝑥 ,𝑥 , 𝑡ˆ ) (𝑥ˆ−𝑥) =ℎ(𝑥 , 𝑡ˆ ) −ℎ(𝑥 , 𝑡). (4.6) We design a nonlinear state estimation law parameterized by a matrix-valued func- tion𝑀(𝑥 , 𝑡ˆ )as follows:

¤ˆ

𝑥 = 𝑓(𝑥 , 𝑡ˆ ) +𝐿(𝑥 , 𝑡ˆ ) (𝑦−ℎ(𝑥 , 𝑡ˆ )) (4.7)

= 𝑓(𝑥 , 𝑡ˆ ) +𝑀(𝑥 , 𝑡ˆ )𝐶¯(𝜚_𝑐,𝑥 , 𝑡ˆ )^⊤𝑅(𝑥 , 𝑡ˆ )⁻¹(𝑦−ℎ(𝑥 , 𝑡ˆ ))

where ¯𝐶(𝜚_𝑐,𝑥 , 𝑡ˆ ) = 𝐶(𝜚_𝑐,𝑥 ,ˆ 𝑥 , 𝑡¯ ) for a fixed trajectory ¯𝑥 (e.g., ¯𝑥 = 0, see Theo- rem 3.2), 𝑅(𝑥 , 𝑡ˆ ) ≻ 0 is a weight matrix on the measurement 𝑦, and 𝑀(𝑥 , 𝑡ˆ ) ≻ 0 is a positive definite matrix (which satisfies the matrix inequality constraint for a contraction metric, to be given in (4.12) of Theorem 4.5). Note that we could use other forms of estimation laws such as the EKF [2], [6], [7], analytical SLAM [8], or SDC with respect to a fixed point [1], [4], [9], depending on the application of interest, which result in a similar stability analysis as in Theorem 3.2.

4.2.I Nonlinear Stability Analysis of SDC-based State Estimation using Con- traction Theory

Substituting (4.7) into (4.3) and (4.4) yields the following virtual system of a smooth path𝑞(𝜇, 𝑡), parameterized by𝜇∈ [0,1]to have𝑞(𝜇=0, 𝑡) =𝑥and𝑞(𝜇=1, 𝑡) =𝑥ˆ:

¤

𝑞(𝜇, 𝑡) =𝜁(𝑞(𝜇, 𝑡), 𝑥 ,𝑥 , 𝑡ˆ ) +𝑑(𝜇, 𝑥 ,𝑥 , 𝑡ˆ ) (4.8) 𝑑𝑞(𝜇, 𝑡) =𝜁(𝑞(𝜇, 𝑡), 𝑥 ,𝑥 , 𝑡ˆ )𝑑 𝑡+𝐺(𝜇, 𝑥 ,𝑥 , 𝑡ˆ )𝑑𝒲(𝑡) (4.9)

where 𝑑(𝜇, 𝑥 ,𝑥 , 𝑡ˆ ) = (1 − 𝜇)𝑑_𝑒0(𝑥 , 𝑡) + 𝜇 𝐿(𝑥 , 𝑡ˆ )𝑑_𝑒1(𝑥 , 𝑡), 𝐺(𝜇, 𝑥 ,𝑥 , 𝑡ˆ ) = [(1 − 𝜇)𝐺_𝑒0(𝑥 , 𝑡), 𝜇 𝐿(𝑥 , 𝑡ˆ )𝐺_𝑒1(𝑥 , 𝑡)],𝒲 = [𝒲^⊤

0 ,𝒲₁^⊤]^⊤, and 𝜁(𝑞, 𝑥 , 𝑥_𝑑, 𝑢_𝑑, 𝑡)is defined as

𝜁(𝑞, 𝑥 ,𝑥 , 𝑡ˆ ) =(𝐴(𝜚_𝑎, 𝑥 ,𝑥 , 𝑡ˆ ) −𝐿(𝑥 , 𝑡ˆ )𝐶(𝜚_𝑐, 𝑥 ,𝑥 , 𝑡ˆ )) (𝑞−𝑥) + 𝑓(𝑥 , 𝑡). (4.10) Note that (4.10) is constructed to contain𝑞 =𝑥ˆand𝑞 =𝑥as its particular solutions of (4.8) and (4.9). If𝑑 =0 and𝒲 =0, the differential dynamics of (4.8) and (4.9) for𝜕_𝜇𝑞=𝜕 𝑞/𝜕 𝜇is given as

𝜕_𝜇𝑞¤ =(𝐴(𝜚_𝑎, 𝑥 ,𝑥 , 𝑡ˆ ) −𝐿(𝑥 , 𝑡ˆ )𝐶(𝜚_𝑐, 𝑥 ,𝑥 , 𝑡ˆ ))𝜕_𝜇𝑞 . (4.11) The similarity between (3.14) (𝜕_𝜇𝑞¤= (𝐴−𝐵𝐾)𝜕_𝜇𝑞) and (4.11) leads to the following theorem [1]–[4]. Again, note that we could also use the SDC formulation with respect to a fixed point as delineated in Theorem 3.2 and as demonstrated in [1], [4], [9].

Theorem 4.3. Suppose ∃𝜌,¯ 𝑐¯ ∈ R^≥0 s.t. ∥𝑅⁻¹(𝑥 , 𝑡ˆ ) ∥ ≤ 𝜌¯, ∥𝐶(𝜚_𝑐, 𝑥 ,𝑥 , 𝑡ˆ ) ∥ ≤

¯

𝑐, ∀𝑥 ,𝑥 , 𝑡ˆ . Suppose also that 𝑚I ⪯ 𝑀 ⪯ 𝑚 𝐼 of (2.26) holds, or equivalently, I ⪯ 𝑊¯ ⪯ 𝜒 𝐼 of(3.19)holds with𝑊 = 𝑀(𝑥 , 𝑡ˆ )⁻¹,𝑊¯ = 𝜈𝑊, 𝜈 =𝑚, and 𝜒 = 𝑚/𝑚. As in Theorem 3.1, let𝛽be defined as𝛽 =0for deterministic systems(4.3)and 𝛽=𝛼_𝑠 =𝛼_𝑒0+𝜈²𝛼_𝑒1 =𝐿_𝑚𝑔¯²

𝑒0(𝛼_𝐺 +1/2)/2+𝜈²𝐿_𝑚𝜌¯²𝑐¯²𝑔¯²

𝑒1(𝛼_𝐺 +1/2)/2 for stochastic systems(4.4), where2𝛼_𝑒0= 𝐿_𝑚𝑔¯²

𝑒0(𝛼_𝐺+1/2),2𝛼_𝑒1= 𝐿_𝑚𝜌¯²𝑐¯²𝑔¯²

𝑒1(𝛼_𝐺+ 1/2), 𝐿_𝑚 is the Lipschitz constant of 𝜕𝑊/𝜕 𝑥_𝑖, 𝑔¯_𝑒0 and 𝑔¯_𝑒1 are given in(4.4), and

∃𝛼_𝐺 ∈R^>0is an arbitrary constant as in Theorem 2.5.

If 𝑀(𝑥 , 𝑡ˆ ) in (4.7) is constructed to satisfy the following convex constraint for

∃𝛼∈R^>0:

¤¯

𝑊+2 sym(𝑊 𝐴¯ −𝜈𝐶¯^⊤𝑅⁻¹𝐶) ⪯ −2𝛼𝑊¯ −𝜈 𝛽I (4.12) then Theorems 2.4 and 2.5 hold for the virtual systems(4.8)and(4.9), respectively, i.e., we have the following bounds fore =𝑥ˆ−𝑥with𝜈 =𝑚and 𝜒=𝑚/𝑚:

∥e(𝑡) ∥ ≤

√

𝑚𝑉_ℓ(0)𝑒^−𝛼𝑡+

¯ 𝑑_𝑒0

√

𝜒+𝜌¯𝑐¯𝑑¯_𝑒1𝜈 𝛼

(1−𝑒^−𝛼𝑡) (4.13)

E

∥e(𝑡) ∥²

≤ 𝑚E[𝑉_𝑠ℓ(0)]𝑒^−2𝛼𝑡+ 𝐶_𝑒0𝜒+𝐶_𝑒1𝜒 𝜈² 2𝛼

(4.14) where 𝑉_𝑠ℓ = ∫𝑥ˆ

𝑥

𝛿𝑞^⊤𝑊 𝛿𝑞 and 𝑉_ℓ = ∫𝑥ˆ

𝑥 ∥Θ𝛿𝑞∥ are given in Theorem 2.3 with 𝑊 = 𝑀⁻¹ = Θ^⊤Θ defining a contraction metric, the disturbance bounds 𝑑¯_𝑒0, 𝑑¯_𝑒1,

¯

𝑔_𝑒0, and 𝑔¯_𝑒1 are given in (4.3)and (4.4), respectively,𝐶_𝑒0 = 𝑔¯²

𝑒0(2𝛼_𝐺⁻¹+1), and 𝐶_𝑒1 = 𝜌¯²𝑐¯²𝑔¯²

𝑒1(2𝛼_𝐺⁻¹+1). Note that for stochastic systems, the probability that

∥e∥ is greater than or equal to𝜀∈R^>0is given as P[∥e(𝑡) ∥ ≥ 𝜀] ≤ 1

𝜀²

𝑚E[𝑉_𝑠ℓ(0)]𝑒^−2𝛼𝑡+ 𝐶_𝐸 2𝛼

(4.15) where𝐶_𝐸 =𝐶_𝑒0𝜒+𝐶_𝑒1𝜒 𝜈².

Proof. Theorem 3.1 indicates that (4.12) is equivalent to

𝑊¤ +2 sym(𝑊 𝐴−𝐶¯^⊤𝑅⁻¹𝐶) ⪯ −2𝛼𝑊−𝛽I. (4.16) Computing the time derivative of a Lyapunov function𝑉 =𝜕_𝜇𝑞^⊤𝑊 𝜕_𝜇𝑞with𝜕_𝜇𝑞=

𝜕 𝑞/𝜕 𝜇for the unperturbed virtual dynamics (4.11), we have using (4.16) that 𝑉¤ =𝜕_𝜇𝑞^⊤𝑊 𝜕_𝜇𝑞 =𝜕_𝜇𝑞^⊤( ¤𝑊 +2𝑊 𝐴−2 ¯𝐶^⊤𝑅⁻¹𝐶)𝜕_𝜇𝑞 ≤ −2𝛼𝑉 −𝛽∥𝜕_𝜇𝑞∥²

which implies that𝑊 = 𝑀⁻¹defines a contraction metric. Since we have 𝑚⁻¹I ⪯ 𝑊 ⪯ 𝑚⁻¹I,𝑉 ≥ 𝑚⁻¹∥𝜕_𝜇𝑞∥², and

∥Θ(𝑥 , 𝑡ˆ )𝜕_𝜇𝑑∥ ≤ 𝑑¯_𝑒0/√

𝑚+𝑑¯_𝑒1𝜌¯𝑐¯

√ 𝑚

∥𝜕_𝜇𝐺∥²_𝐹 ≤ 𝑔¯²

𝑒0+𝜌¯²𝑐¯²𝑔¯²

𝑒1𝑚²

for 𝑑 in (4.8) and𝐺 in (4.9), the bounds (4.13) – (4.15) follow from the proofs of Theorems 2.4 and 2.5 [2], [3].

Remark 4.1. Although (4.12) is not an LMI due to the nonlinear term −𝜈 𝛽I on its right-hand side for stochastic systems(4.4), it is a convex constraint as−𝜈 𝛽 =

−𝜈𝛼_𝑠 =−𝜈𝛼_𝑒0−𝜈³𝛼_𝑒1is a concave function for𝜈 ∈R^>0[3], [10].

4.2.II CV-STEM Formulation for State Estimation

The estimator (4.7) gives a convex steady-state upper bound of the Euclidean distance between𝑥and ˆ𝑥 as in Theorem 4.1 [1]–[4].

Theorem 4.4. If(4.12)of Theorem 4.3 holds, then we have the following bound:

𝑡→∞lim

√︃

E

∥𝑥ˆ−𝑥∥²

≤ 𝑐₀(𝛼, 𝛼_𝐺)𝜒+𝑐₁(𝛼, 𝛼_𝐺)𝜈^𝑠 (4.17) where 𝑐₀ = 𝑑¯_𝑒0/𝛼, 𝑐₁ = 𝜌¯𝑐¯𝑑¯_𝑒1/𝛼, 𝑠 = 1 for deterministic systems (4.8), and 𝑐₀=√︁

𝐶_𝑒0/(2𝛼),𝑐₁=𝐶_𝑒1/(2√

2𝛼𝐶_𝑒0), and𝑠=2for stochastic systems(4.9), with 𝐶_𝑒0and𝐶_𝑒0given as𝐶_𝑒0=𝑔¯²

𝑒0(2𝛼⁻¹

𝐺 +1)and𝐶_𝑒1= 𝜌¯²𝑐¯²𝑔¯²

𝑒1(2𝛼⁻¹

𝐺 +1).

Proof. The upper bound (4.17) for deterministic systems (4.8) follows from (4.13) with the relation 1 ≤ √

𝜒 ≤ 𝜒due to𝑚 ≤ 𝑚. For stochastic systems, we have using (4.14) that

𝐶_𝑒0𝜒+𝐶_𝑒1𝜈²𝜒 ≤ 𝐶_𝑒0(𝜒+ (𝐶_𝑒1/(2𝐶_𝑒0))𝜈²)²

due to 1≤ 𝜒 ≤ 𝜒²and𝜈 ∈R^>0. This gives (4.17) for stochastic systems (4.9).

Finally, the CV-STEM estimation framework is summarized in Theorem 4.5 [1]–[4].

Theorem 4.5. Suppose that𝛼, 𝛼_𝐺, 𝑑¯_𝑒0, 𝑑¯_𝑒1, 𝑔¯_𝑒0, 𝑔¯_𝑒1, and𝐿_𝑚 in(4.12) and(4.17) are given. If the pair (𝐴, 𝐶) is uniformly observable, the non-convex optimization problem of minimizing the upper bound(4.17)is equivalent to the following convex optimization problem with the contraction constraint (4.12) and I ⪯ 𝑊¯ ⪯ 𝜒I of (3.19):

𝐽^∗

𝐶𝑉 = min

𝜈∈R^>0, 𝜒∈R,𝑊¯≻0

𝑐₀𝜒+𝑐₁𝜈^𝑠+𝑐₂𝑃(𝜒, 𝜈,𝑊¯) (4.18) s.t.(4.12) and (3.19)

where𝑐₀,𝑐₁, and𝑠are as defined in(4.17)of Theorem 4.4,𝑐₂ ∈R≥0, and𝑃is some performance-based cost function as in Theorem 4.2.

The weight 𝑐₁ for 𝜈^𝑠 indicates how much we trust the measurement 𝑦(𝑡). Using non-zero𝑐₂enables finding contraction metrics optimal in a different sense in terms of 𝑃. Furthermore, the coefficients of the SDC parameterizations 𝜚_𝑎 and 𝜚_𝑐 in Lemma 3.1 (i.e., 𝐴 = Í

𝜚_𝑎,𝑖𝐴_𝑖 and𝐶 = Í

𝜚_𝑐,𝑖𝐶_𝑖 in (4.5) and (4.6)) can also be treated as decision variables by convex relaxation [9], thereby adding a design flexibility to mitigate the effects of external disturbances while verifying the system observability.

Proof. The proposed optimization problem is convex as its objective and constraints are convex in terms of decision variables 𝜒, 𝜈, and ¯𝑊 (see Remark 4.1). Also, larger ¯𝑑_𝑒1and ¯𝑔_𝑒1in (4.3) and (4.4) imply larger measurement uncertainty. Thus by definition of𝑐₁in Theorem 4.4, the larger the weight of𝜈, the less confident we are in 𝑦(𝑡)(see Example 4.2). The last statement on the SDC coefficients for guaranteeing observability follows from Proposition 1 of [9] and Proposition 1 of [1].

Example 4.2. The weights 𝑐₀ and𝑐₁ of the CV-STEM estimation of Theorem 4.5 has an analogous trade-off to the case of the Kalman filter with the process and

sensor noise covariance matrices,𝑄and𝑃, respectively, since the term𝑐₀𝜒in upper bound of the steady-state tracking error in(4.17)becomes dominant if measurement noise is much smaller than process noise (𝑑¯_𝑒0 ≫ 𝑑¯_𝑒1 or𝑔¯_𝑒0 ≫ 𝑔¯_𝑒1), and the term 𝑐₁𝜈^𝑠 becomes dominant if measurement noise is much greater than process noise (𝑑¯_𝑒0 ≪ 𝑑¯_𝑒1or𝑔¯_𝑒0≪ 𝑔¯_𝑒1). In particular [2], [3],

• if𝑐₁is much greater than𝑐₀, large measurement noise leads to state estimation that responds slowly to unexpected changes in the measurement 𝑦 (i.e. small estimation gain due to𝜈 =𝑚 ≥ ∥𝑀∥), and

• if 𝑐₁is much smaller than𝑐₀, large process noise leads to state estimation that responds fast to changes in the measurement (i.e. large𝜈 =𝑚 ≥ ∥𝑀∥).

This is also because the solution 𝑄 = 𝑃⁻¹ ≻ 0of the Kalman filter Riccati equa- tion [7, p. 375], 𝑃¤ = 𝐴𝑃+𝑃 𝐴^⊤ −𝑃𝐶^⊤𝑅⁻¹𝐶 𝑃+𝑄, can be viewed as a positive definite matrix that defines a contraction metric as discussed in Example 2.3.