Two-Stage Algorithm for Estimation of Nonlinear Functions of State Vector in Linear Gaussian Continuous Dynamical Systems

Abstract

This paper focuses on the optimal minimum mean square error estimation of a nonlinear function of state (NFS) in linear Gaussian continuous-time stochastic systems. The NFS represents a multivariate function of state variables which carries useful information of a target system for control. The main idea of the proposed optimal estimation algorithm includes two stages: the optimal Kalman estimate of a state vector computed at the first stage is nonlinearly transformed at the second stage based on the NFS and the minimum mean square error (MMSE) criterion. Some challenging theoretical aspects of analytic calculation of the optimal MMSE estimate are solved by usage of the multivariate Gaussian integrals for the special NFS such as the Euclidean norm, maximum and absolute value. The polynomial functions are studied in detail. In this case the polynomial MMSE estimator has a simple closed form and it is easy to implement in practice. We derive effective matrix formulas for the true mean square error of the optimal and suboptimal quadratic estimators. The obtained results we demonstrate on theoretical and practical examples with different types of NFS. Comparison analysis of the optimal and suboptimal nonlinear estimators is presented. The subsequent application of the proposed estimators demonstrates their effectiveness.

APPENDIX

Proof of Theorem 1. The derivation of the polynomial estimators (3.2) is based on the Lemma 1.

Lemma 1. Let $x \in {{\mathbb{R}}^{n}}$ be a Gaussian random vector, $x \sim \mathbb{N}(\mu ,S)$ and $A,B \in {{\mathbb{R}}^{{n \times n}}}$ be an arbitrary matrices. Then it holds that

$$\begin{gathered} {\mathbf{E}}(Ax) = A\mu ; \\ {\mathbf{E}}({{x}^{{\text{T}}}}Ax) = {\text{tr}}(AS) + {{\mu }^{{\text{T}}}}A\mu ,\quad A = {{A}^{{\text{T}}}}; \\ {\mathbf{E}}(Ax{{x}^{{\text{T}}}}Bx) = 2ASB\mu + A\mu {{\mu }^{{\text{T}}}}B\mu + A\mu {\text{tr}}(BS); \\ E({{x}^{T}}Ax{{x}^{T}}Bx) = 2tr(ASBS) + 4{{\mu }^{T}}ASB\mu \\ \, + [{\text{tr}}(AS) + {{\mu }^{T}}A\mu {\text{][tr}}(BS) + {{\mu }^{T}}B\mu ],\quad A = {{A}^{{\text{T}}}},\quad B = {{B}^{{\text{T}}}}. \\ \end{gathered} $$

((A.1))

The derivation of the formulas (A.1) is based on their scalar versions given in [37, 38], and standard transformations on random vectors.

This completes the proof of Lemma 1.

Next, replacing in (A.1) an unconditional expectations and covariance by their conditional versions, for example, $\mu \to {\mathbf{E}}(x {\text{|}}{\kern 1pt} y_{0}^{t}) = \hat {x}$, S → ${\text{cov(}}\hat {x},\hat {x} {\text{|}}{\kern 1pt} y_{0}^{t}{\text{)}}$ = P we obtain (3.2).

This completes the proof of Theorem 1.

Proof of Theorem 2. The derivation of the MSEs is based on the Lemma 2.

Lemma 2.Let $X \in {{\mathbb{R}}^{{3n}}}$ be a composite multivariate Gaussian vector, ${{X}^{{\text{T}}}} = [{{U}^{{\text{T}}}}\;{{V}^{T}}\;{{W}^{T}}]$:

$$\begin{gathered} X\sim \mathbb{N}(\mu ,S),\quad U,V,W \in {{\mathbb{R}}^{n}}, \\ \mu = \left[ \begin{gathered} {{\mu }_{u}} \\ {{\mu }_{{v}}} \\ {{\mu }_{w}} \\ \end{gathered} \right],\quad S = \left[ {\begin{array}{*{20}{c}} {{{S}_{{uu}}}}&{{{S}_{{u{v}}}}}&{{{S}_{{uw}}}} \\ {{{S}_{{{v}u}}}}&{{{S}_{{{vv}}}}}&{{{S}_{{{v}w}}}} \\ {{{S}_{{wu}}}}&{{{S}_{{w{v}}}}}&{{{S}_{{ww}}}} \end{array}} \right]. \\ \end{gathered} $$

((A.2))

Then the third- and fourth-order vector moments of the composite random vector are given by

$$\begin{gathered} {\mathbf{E}}({{U}^{{\text{T}}}}VW) = \mu _{u}^{{\text{T}}}{{\mu }_{{v}}}{{\mu }_{w}} + {\text{tr}}({{S}_{{u{v}}}})\mu _{w}^{{\text{T}}} + \mu _{{v}}^{{\text{T}}}{{S}_{{uw}}} + \mu _{u}^{{\text{T}}}{{S}_{{{v}w}}}; \\ {\mathbf{E}}({{U}^{{\text{T}}}}U{{V}^{{\text{T}}}}V) = \mu _{u}^{{\text{T}}}{{\mu }_{u}}\mu _{{v}}^{{\text{T}}}{{\mu }_{{v}}} + 2{\text{tr}}({{S}_{{u{v}}}}{{S}_{{{v}u}}}) + {\text{tr}}({{S}_{{uu}}}){\text{tr}}({{S}_{{{vv}}}}) \\ \, + {\text{tr}}({{S}_{{uu}}})\mu _{{v}}^{{\text{T}}}{{\mu }_{{v}}} + {\text{tr}}({{S}_{{{vv}}}})\mu _{u}^{{\text{T}}}{{\mu }_{u}} + 4\mu _{u}^{{\text{T}}}{{S}_{{u{v}}}}{{\mu }_{{v}}}; \\ {\mathbf{E}}({{U}^{{\text{T}}}}V{{V}^{{\text{T}}}}U) = \mu _{u}^{{\text{T}}}{{\mu }_{{v}}}\mu _{{v}}^{{\text{T}}}{{\mu }_{u}} + {\text{tr}}({{S}_{{uu}}}{{S}_{{{vv}}}}) + {\text{tr}}({{S}_{{u{v}}}}){\text{tr}}({{S}_{{{v}u}}}) \\ \, + {\text{tr}}(S_{{u{v}}}^{2}) + \mu _{{v}}^{{\text{T}}}{{S}_{{uu}}}{{\mu }_{{v}}} + \mu _{u}^{{\text{T}}}{{S}_{{{vv}}}}{{\mu }_{u}} + \mu _{u}^{{\text{T}}}{{S}_{{u{v}}}}{{\mu }_{u}} + \mu _{u}^{{\text{T}}}{{S}_{{{v}u}}}{{\mu }_{{v}}} + 2{\text{tr}}({{S}_{{u{v}}}})\mu _{u}^{{\text{T}}}{{\mu }_{{v}}}; \\ {\mathbf{E}}({{U}^{{\text{T}}}}V{{W}^{{\text{T}}}}U) = \mu _{u}^{{\text{T}}}{{\mu }_{{v}}}\mu _{w}^{{\text{T}}}{{\mu }_{u}} + {\text{tr}}({{S}_{{u{v}}}}){\text{tr}}({{S}_{{uw}}}) + {\text{tr}}({{S}_{{uu}}}{{S}_{{w{v}}}}) \\ \, + {\text{tr}}({{S}_{{uw}}}{{S}_{{u{v}}}}) + {\text{tr}}({{S}_{{u{v}}}})\mu _{u}^{{\text{T}}}{{\mu }_{w}} + {\text{tr}}({{S}_{{uw}}})\mu _{u}^{{\text{T}}}{{\mu }_{{v}}} \\ \, + \mu _{{v}}^{{\text{T}}}{{S}_{{uu}}}{{\mu }_{w}} + \mu _{{v}}^{{\text{T}}}{{S}_{{uw}}}{{\mu }_{u}} + \mu _{u}^{{\text{T}}}{{S}_{{{v}u}}}{{\mu }_{w}} + \mu _{u}^{{\text{T}}}{{S}_{{{v}w}}}{{\mu }_{u}}. \\ \end{gathered} $$

((A.3))

The derivation of the vector formulas (A.3) is based on their scalar versions, and standard matrix manipulations,

$$\begin{gathered} {\mathbf{E}}\left( {{{x}_{i}}{{x}_{j}}{{x}_{k}}} \right) = {{\mu }_{i}}{{\mu }_{j}}{{\mu }_{k}} + {{\mu }_{i}}{{S}_{{jk}}} + {{\mu }_{j}}{{S}_{{ik}}} + {{\mu }_{k}}{{S}_{{ij}}};~~~ \\ {\mathbf{E}}\left( {{{x}_{i}}{{x}_{j}}{{x}_{k}}{{x}_{\ell }}} \right) = {{\mu }_{i}}{{\mu }_{j}}{{\mu }_{k}}{{\mu }_{\ell }} + {{S}_{{ij}}}{{S}_{{k\ell }}} + {{S}_{{ik}}}{{S}_{{\ell j}}} + {{S}_{{i\ell }}}{{S}_{{jk}}} + {{\mu }_{i}}{{\mu }_{j}}{{S}_{{k\ell }}} + {{\mu }_{i}}{{\mu }_{k}}{{S}_{{j\ell }}} + {{\mu }_{i}}{{\mu }_{\ell }}{{S}_{{jk}}} \\ \, + \;~{{\mu }_{j}}{{\mu }_{k}}{{S}_{{i\ell }}} + {{\mu }_{j}}{{\mu }_{\ell }}{{S}_{{ik}}} + {{\mu }_{k}}{{\mu }_{\ell }}{{S}_{{ij}}},~ \\ \end{gathered} $$

((A.4))

where ${{\mu }_{h}} = {\mathbf{E}}({{x}_{h}})$, ${{S}_{{pq}}} = {\text{cov(}}{{x}_{p}},{{x}_{q}}{\text{)}}$.

This completes the proof of Lemma 2.

Further, we derive the formula (3.8). Using (3.5) and (3.6), the error can be written as

$$\begin{gathered} {{e}_{z}} = z - \hat {z} = {{x}^{{\text{T}}}}Ax - {\text{tr}}\left( {AP} \right) - {{{\hat {x}}}^{{\text{T}}}}A\hat {x} = {{\left( {e + \hat {x}} \right)}^{{\text{T}}}}A\left( {e + \hat {x}} \right) - {{{\hat {x}}}^{{\text{T}}}}A\hat {x} - {\text{tr}}\left( {AP} \right)~ \\ \; = {{e}^{{\text{T}}}}Ae + 2{{e}^{{\text{T}}}}A\hat {x} - {\text{tr}}\left( {AP} \right),\quad e = x - \hat {x},\quad {{{\hat {x}}}^{{\text{T}}}}Ae = {{e}^{{\text{T}}}}A\hat {x}. \\ \end{gathered} $$

((A.5))

Next, using the unbiased and orthogonality properties of the Kalman estimate ${\mathbf{E}}\left( e \right) = {\mathbf{E}}(e{{\hat {x}}^{{\text{T}}}}) = 0,$ we obtain

$$\begin{gathered} P_{z}^{{{\text{opt}}}} = {\mathbf{E}}({{e}^{{\text{T}}}}Ae{{e}^{{\text{T}}}}Ae) + 2{\mathbf{E}}({{e}^{{\text{T}}}}A\hat {x}{{e}^{{\text{T}}}}Ae) \\ \, - {\text{tr}}(AP){\mathbf{E}}({{e}^{{\text{T}}}}Ae) + 2{\mathbf{E}}({{e}^{{\text{T}}}}Ae{{e}^{{\text{T}}}}A\hat {x}) \\ \, + 4{\mathbf{E}}({{e}^{{\text{T}}}}A\hat {x}{{e}^{{\text{T}}}}A\hat {x}) - 2{\text{tr}}(AP){\mathbf{E}}({{e}^{{\text{T}}}}A\hat {x}) \\ \, - {\text{tr}}(AP){\mathbf{E}}({{e}^{{\text{T}}}}Ae) - 2{\text{tr}}(AP){\mathbf{E}}({{e}^{{\text{T}}}}A\hat {x}) + {\text{t}}{{{\text{r}}}^{2}}(AP). \\ \end{gathered} $$

((A.6))

Using Lemma 2 we can calculate high-order moments in (A.6). We have

$$\begin{gathered} {\mathbf{E}}({{e}^{{\text{T}}}}Ae) = {\text{tr}}\left( {AP} \right),~~~{\mathbf{E}}({{e}^{{\text{T}}}}A\hat {x}) = 0,\quad {\mathbf{E}}({{e}^{{\text{T}}}}Ae{{e}^{{\text{T}}}}Ae) = {\text{t}}{{{\text{r}}}^{2}}\left( {AP} \right) + 2{\text{tr}}\left( {APAP} \right), \\ {\mathbf{E}}({{e}^{{\text{T}}}}A\hat {x}{{e}^{{\text{T}}}}Ae) = {\mathbf{E}}({{e}^{{\text{T}}}}Ae{{e}^{{\text{T}}}}A\hat {x}) = 0,\quad {\mathbf{E}}({{e}^{{\text{T}}}}A\hat {x}{{e}^{{\text{T}}}}A\hat {x}) = {\mathbf{E}}({{e}^{{\text{T}}}}A\hat {x}{{{\hat {x}}}^{{\text{T}}}}Ae) = {\text{tr}}\left( {PA{{C}_{{\hat {x}\hat {x}}}}A} \right) + {{\mu }^{{\text{T}}}}AP\mu , \\ \end{gathered} $$

((A.7))

where

$$\begin{gathered} \mu = {\mathbf{E}}\left( x \right) = {\mathbf{E}}\left( {\hat {x}} \right),\quad {\text{cov}}\left( {Ae,Ae} \right) = AP{{A}^{{\text{T}}}}, \\ {{C}_{{\hat {x}\hat {x}}}} = C - P,~~~P = {\text{cov}}\left( {e,e} \right),\quad C = {\text{cov}}\left( {x,x} \right). \\ \end{gathered} $$

((A.8))

Substituting (A.7) in (A.6), and after some manipulations, we get the optimal MSE (3.8).

In the case of the suboptimal estimate $\tilde {z},$ the derivation of the MSE (3.9) is similar.

This completes the proof of Theorem 2.