Approximating arrival costs in distributed moving horizon estimation: A recursive method

$\text{diag}\left\{A_{1},\ldots,A_{n}\right\}$ represents the block diagonal matrix with blocks $A_{i}$, $i=1,\ldots,n$. $[A_{ij}]$ represents a block matrix where the $A_{ij}$ is the submatrix in the $i$th row and the $j$th column. $I_{n}$ is an $n\times n$ identity matrix. $\|z\|_{A}^{2}=z^{\mathrm{T}}Az$ is the square of the weighted Euclidean norm of vector $z$. $\text{col}\{z_{1},\ldots,z_{n}\}$ denotes a column vector containing a sequence $z_{1},\ldots,z_{n}$. $\{z\}_{a}^{b}=\text{col}\{z_{a},z_{a+1},\ldots,z_{b}\}$.

Let us consider nonlinear systems consisting of $n$ interconnected subsystems. The dynamics of the $i$th subsystem, $i\in\mathbb{I}=\{1,2,\ldots,n\}$, are expressed as follows:

	$\displaystyle x^{i}_{k+1}$	$\displaystyle=f_{i}\big{(}x^{i}_{k},X^{i}_{k}\big{)}+w^{i}_{k}$		(1a)
	$\displaystyle y^{i}_{k}$	$\displaystyle=h_{i}(x^{i}_{k})+v^{i}_{k}$		(1b)

where $k$ denotes discrete-sampling instant; $x^{i}_{k}\in\mathbb{R}^{n_{x^{i}}}$ and $y^{i}_{k}\in\mathbb{R}^{n_{y^{i}}}$ represent the state vector and output measurements of the $i$th subsystem, respectively; $X^{i}_{k}\in\mathbb{R}^{n_{X^{i}}}$ is a vector of the states of all the subsystems that have direct influence on the dynamics of subsystem $i$; $w^{i}_{k}\in\mathbb{R}^{n_{x^{i}}}$ and $v^{i}_{k}\in\mathbb{R}^{n_{y^{i}}}$ represent the unknown process disturbances and measurement noise associated with subsystem $i$, respectively; $f_{i}$ is a vector-value nonlinear function characterizing the dynamics of subsystem $i$; $h_{i}$ is the output measurement function for subsystem $i$, $i\in\mathbb{I}$.

By considering the variables for the entire system as the aggregation of the variables for each subsystem $i$, $i\in\mathbb{I}$: $z_{k}={\rm{col}}\{z^{1}_{k},\ldots,z^{n}_{k}\}\in\mathbb{R}^{n_{z}}$ for vector $z\in\{x,y,w,v\}$, the dynamics of the entire system can be formulated in the following compact form:

	$\displaystyle x_{k+1}$	$\displaystyle=f(x_{k})+w_{k}$		(2a)
	$\displaystyle y_{k}$	$\displaystyle=h(x_{k})+v_{k}$		(2b)

where $f(x_{k})$ and $h(x_{k})$ can be obtained from $f_{i}(x_{k},X^{i}_{k})$ and $h_{i}(x^{i}_{k})$, respectively.

In this work, we aim to propose a partition-based DMHE algorithm for nonlinear systems, which integrates a recursive update mechanism for the arrival cost of each local estimator of the distributed scheme. To achieve this goal, we first present a distributed full-information estimation (FIE) design by partitioning the objective function of a centralized FIE problem. From this design, we derive the analytical expression of the arrival cost for the proposed DMHE method in a linear context. Subsequently, we formulate the proposed DMHE method based on the obtained arrival costs, and sufficient conditions are provided to guarantee the stability of the proposed DMHE approach under state constraints. Finally, the obtained arrival costs for linear systems are extended to account for the nonlinear systems, and a partition-based DMHE algorithm for constrained nonlinear context is formulated.

Based on the above consideration, first, we start by examining a class of linear systems consisting of $n$ subsystems. The dynamics of $i$th linear subsystem, $i\in\mathbb{I}$, are described as follows:

	$\displaystyle x^{i}_{k+1}$	$\displaystyle=A_{ii}x^{i}_{k}+\sum_{l\in\mathbb{I}\setminus\{i\}}A_{il}x^{l}_{k}+w^{i}_{k}$		(3a)
	$\displaystyle y^{i}_{k}$	$\displaystyle=C_{ii}x^{i}_{k}+v^{i}_{k}$		(3b)

where $A_{ii}$, $A_{il}$, and $C_{ii}$, $i\in\mathbb{I}$, $l\in\mathbb{I}\setminus\{i\}$, are subsystem matrices of compatible dimensions.

Based on the linear subsystem models in (3), we will design MHE-based local estimators. These estimators will then be integrated to formulate a linear DMHE design, which will be extended to account for state estimation of nonlinear systems in (1). A compact form of the linear system comprising all the subsystems in the form of (3) is described as follows:

	$\displaystyle x_{k+1}$	$\displaystyle=Ax_{k}+w_{k}$		(4a)
	$\displaystyle y_{k}$	$\displaystyle=Cx_{k}+v_{k}$		(4b)

where $A=[A_{il}]$ represents a block matrix where $A_{il}$ is the submatrix in the $i$th row and the $l$th column; $C=\mathrm{diag}\{C_{11},\ldots,C_{nn}\}$.

Full-information estimation (FIE) is an optimization-based state estimation approach, which can be viewed as a least-squares estimation method that minimizes the cumulative sum of squared errors from the initial time instant to the current time instant. Specifically, at each sampling instant $k$, based on the linear model in (4), a centralized FIE design can be formulated as follows [26]:

	$\displaystyle\min_{\{\hat{x}_{j}\}_{j=0}^{j=k}}\bar{\Phi}_{k}=\sum_{j=0}^{k-1}\|\hat{w}_{j}\|^{2}_{Q^{-1}}+\sum_{j=0}^{k}\|\hat{v}_{j}\|^{2}_{R^{-1}}+\|\hat{x}_{0}-\bar{x}_{0}\|_{P_{0}^{-1}}^{2}$
	$\displaystyle\quad\text{s.t.}~{}~{}~{}~{}~{}\hat{x}_{j+1}=A\hat{x}_{j}+\hat{w}_{j}$		(5a)
	$\displaystyle\quad\quad\quad~{}~{}~{}~{}~{}~{}\,y_{j}=C\hat{x}_{j}+\hat{v}_{j}$		(5b)

where $\hat{x}_{j}$ is an estimate of state $x_{j}$; $\bar{x}_{0}$ is an a priori estimate of initial state $x_{0}$; $\hat{w}_{j}$ and $\hat{v}_{j}$ are estimates of process disturbances $w_{j}$ and measurement noise $v_{j}$, respectively; $P_{0}$, $Q$, and $R$ are positive-definite weighting matrices. It is noted that as the number of sampling instants increases over time, the associated optimization problem in (5) becomes increasingly complex and intractable. FIE is important for the design and analysis of the MHE approaches [26, 27]. In the next section, we introduce a distributed FIE design, which is used to guide the development of the DMHE approach.

Following [24], the global objective function of the centralized FIE algorithm $\bar{\Phi}_{k}$ is decomposed into $\bar{\Phi}^{i}_{k}$, $i\in\mathbb{I}$, such that $\bar{\Phi}_{k}=\sum_{i\in\mathbb{I}}\bar{\Phi}^{i}_{k}$. The objective function of each local estimator is described as follows:

$\displaystyle\bar{\Phi}^{i}_{k}$

$\displaystyle=\sum_{j=0}^{k-1}\|\hat{w}^{i}_{j}\|^{2}_{Q_{i}^{-1}}+\sum_{j=0}^{k}\|\hat{v}^{i}_{j}\|^{2}_{R_{i}^{-1}}+\|\hat{x}^{i}_{0}-\bar{x}^{i}_{0}\|_{P_{i,0}^{-1}}^{2}$

(6)

where $\hat{x}^{i}_{0}$ is an estimate of the $i$th subsystem state $x^{i}_{0}$; $\bar{x}^{i}_{0}$ is an initial guess of $x^{i}_{0}$; $\hat{w}^{i}_{j}$ and $\hat{v}^{i}_{j}$ are the estimates of disturbances $w^{i}_{j}$ and measurement noise $v^{i}_{j}$ of the $i$th subsystem, respectively. It is noted that the sensor measurements from the interconnected subsystems can provide valuable information for estimating the local subsystem states. Inspired by the objective function designs for the distributed state estimation algorithms proposed in [8, 10, 24], each local objective function of the local estimator of distributed FIE is presented as follows:

	$\displaystyle\Phi_{k}^{i}$	$\displaystyle=\bar{\Phi}_{k}^{i}+\sum_{l\in\mathbb{I}\setminus\{i\}}\sum_{j=0}^{k}\|\hat{v}_{j}^{l}\|^{2}_{R_{l}^{-1}}$
		$\displaystyle=\sum_{j=0}^{k-1}\|\hat{w}^{i}_{j}\|^{2}_{Q_{i}^{-1}}+\sum_{j=0}^{k}\|\hat{v}^{[i]}_{j}\|^{2}_{R^{-1}}+\|\hat{x}^{i}_{0}-\bar{x}^{i}_{0}\|_{P_{i,0}^{-1}}^{2}$		(7)

where $\hat{v}^{[i]}_{j}$ is an estimate of measurement noise $v_{j}$ of the entire system within estimator $i$; $P_{i,0}$, $Q_{i}$, and $R_{i}$ are the $i$th diagonal block of weighting matrices $P_{0}$, $Q$, and $R$, respectively. The key difference between $\bar{\Phi}_{k}^{i}$ in (6) and $\Phi_{k}^{i}$ in (3.1) is that $\Phi_{k}^{i}$ includes additional information on the sensor measurements of interconnected subsystems, which can provide valuable insights for the reconstruction of the local subsystem states.

At sampling instant $k$, the local estimator for the $i$th subsystem of distributed FIE can be formulated by leveraging the local objective function proposed in (3.1):

	$\displaystyle\min_{\{\hat{x}^{i}_{j}\}_{j=0}^{j=k}}\Phi^{i}_{k}=\sum_{j=0}^{k-1}\|\hat{w}^{i}_{j}\|^{2}_{Q_{i}^{-1}}+\sum_{j=0}^{k}\|\hat{v}^{[i]}_{j}\|^{2}_{R^{-1}}+\|\hat{x}^{i}_{0}-\bar{x}^{i}_{0}\|_{P_{i,0}^{-1}}^{2}$
	$\displaystyle~{}\,\mathrm{s.t.}~{}\hat{x}_{j+1}^{i}=A_{ii}\hat{x}_{j}^{i}+\sum_{l\in\mathbb{I}\setminus\{i\}}A_{il}\tilde{x}_{j}^{l}+\hat{w}_{j}^{i}$		(8a)
	$\displaystyle~{}\,\quad~{}~{}\quad\,y_{0}=C_{[:,i]}\hat{x}_{0}^{i}+\sum_{l\in\mathbb{I}\setminus\{i\}}C_{[:,l]}\tilde{x}_{0}^{l}+\hat{v}_{0}^{[i]}$		(8b)
	$\displaystyle~{}\,\quad~{}~{}~{}y_{j+1}=C(A_{[:,i]}\hat{x}_{j}^{i}+\sum_{l\in\mathbb{I}\setminus\{i\}}A_{[:,l]}\tilde{x}_{j}^{l})+v_{j+1}^{[i]},~{}j=0,\ldots,k-1$		(8c)

where $A_{[:,i]}$ and $C_{[:,i]}$ comprise the columns of $A$ and $C$ with respect to the state of $x^{i}$, respectively; $\tilde{x}_{j}^{l}$ is a (conservative) state estimate of subsystem $l$ for sampling instant $j$ made available to the estimator of subsystem $i$, which is determined as:

$$\normalsize\tilde{x}_{j}^{l}=\left\{\begin{array}[]{l}\hat{x}_{j|k-1}^{l},~{}~{}~{}~{}~{}{\text{for}}~{}j=1,\ldots,k-1{}{}\\[3.00003pt] \bar{x}_{0}^{l},~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}{\text{for}}~{}j=0{}{}\end{array}\right.$$

(9)

In (9), $\hat{x}_{j|k-1}^{l}$ is the state estimate of $x^{l}_{j}$ calculated by the $l$th estimator at sampling instant $k-1$, and $\bar{x}_{0}^{l}$ denotes an initial guess of $x_{0}^{l}$, $l\in\mathbb{I}\setminus\{i\}$. We utilize the state estimates $\hat{x}_{j|k-1}^{l}$ obtained at the previous time instant $k-1$ because they are calculated based on the most recent available sensor measurements. In the remainder of this paper, we simplify the subscript by omitting “$|k$”, and we denote $\hat{x}_{j|k}^{i}$ by $\hat{x}_{j}^{i}$ for brevity.

By comparing the design of the distributed FIE in (8) with the proposed DMHE in (10), the arrival cost $V_{k-N}^{i,o}$ can be constructed by deriving the analytical solution to the following optimization problem:

$$V_{k-N}^{i,o}=\min_{\{\hat{x}_{j}^{i}\}_{j=0}^{k-N-1}}V_{k-N}^{i}~{}\text{subject to \eqref{eq:upd0_2}, \eqref{eq:upd0_4}, and \eqref{eq:upd0_3}}$$

where

$\displaystyle V_{k-N}^{i}$

$\displaystyle=\|\hat{x}_{0}^{i}-\bar{x}_{0}^{i}\|^{2}_{P_{i,0}^{-1}}+\sum_{j=0}^{k-N-1}\|\hat{w}^{i}_{j}\|^{2}_{Q_{i}^{-1}}+\sum_{j=0}^{k-N-1}\|\hat{v}_{j}^{[i]}\|^{2}_{R^{-1}}$

First, we consider the arrival cost $V_{1}^{i,o}$, which can be formulated by deriving the analytical solution to the optimization problem below:

	$$V_{1}^{i,o}=\min_{\hat{x}_{0}^{i}}V_{1}^{i}~{}\text{subject to \eqref{eq:upd0_2} and \eqref{eq:upd0_4} for}~{}j=0$$			(11a)
where
	$\displaystyle V_{1}^{i}$	$\displaystyle=\|\hat{x}_{0}^{i}-\bar{x}_{0}^{i}\|^{2}_{P_{i,0}^{-1}}+\|\hat{v}_{0}^{[i]}\|^{2}_{R^{-1}}+\|\hat{w}_{0}^{i}\|^{2}_{Q_{i}^{-1}}$		(11b)

Considering (8b) and Lemma 1, $V_{1}^{i}$ can be rewritten as

	$\displaystyle V^{i}_{1}$	$\displaystyle=\|\hat{x}_{0}^{i}-\bar{x}_{0}^{i}\|^{2}_{P_{i,0}^{-1}}+\|y_{0}-C_{[:,i]}\hat{x}_{0}^{i}-\sum_{l\in\mathbb{I}\setminus\{i\}}C_{[:,l]}\tilde{x}_{0}^{l}\|^{2}_{R^{-1}}+\|\hat{w}_{0}^{i}\|^{2}_{Q_{i}^{-1}}$
		$\displaystyle=\|\hat{x}_{0}^{i}-\breve{x}_{0}^{i}\|^{2}_{\breve{P}_{i,0}^{-1}}+\pi^{i}_{0}+\|\hat{w}_{0}^{i}\|^{2}_{Q_{i}^{-1}}$		(12)

where

	$\displaystyle\breve{P}_{i,0}$	$\displaystyle=P_{i,0}-P_{i,0}C_{[:,i]}^{\mathrm{T}}(C_{[:,i]}P_{i,0}C_{[:,i]}^{\mathrm{T}}+R)^{-1}C_{[:,i]}P_{i,0}$
	$\displaystyle\breve{x}_{0}^{i}$	$\displaystyle=\bar{x}_{0}^{i}+P_{i,0}C_{[:,i]}^{\mathrm{T}}(C_{[:,i]}P_{i,0}C_{[:,i]}^{\mathrm{T}}+R)^{-1}(y_{0}-C_{[:,i]}\bar{x}_{0}^{i}-\sum_{l\in\mathbb{I}\setminus\{i\}}C_{[:,l]}\tilde{x}_{0}^{l})$
	$\displaystyle\pi^{i}_{0}$	$\displaystyle=\|\breve{x}_{0}^{i}-\bar{x}_{0}^{i}\|^{2}_{P_{i,0}^{-1}}+\|y_{0}-C_{[:,i]}\breve{x}_{0}^{i}-\sum_{l\in\mathbb{I}\setminus\{i\}}C_{[:,l]}\tilde{x}_{0}^{l}\|^{2}_{R^{-1}}$

As $\pi^{i}_{0}$ is a constant, we do not need to take it into account when solving the optimization problem (11). According to (8a), Lemma 1, and Lemma 2, it is further obtained that

	$\displaystyle V^{i}_{1}$	$\displaystyle=\|\hat{x}_{0}^{i}-\breve{x}_{0}^{i}\|^{2}_{\breve{P}_{i,0}^{-1}}+\|\hat{x}_{1}^{i}-A_{ii}\hat{x}_{0}^{i}-\sum_{l\in\mathbb{I}\setminus\{i\}}A_{il}\tilde{x}_{0}^{l}\|^{2}_{Q_{i}^{-1}}$
		$\displaystyle=\|\hat{x}_{0}^{i}-\breve{\sigma}^{i}_{0}\|^{2}_{H_{0}^{i}}+\breve{\pi}^{i}_{0}$		(13)

where

	$\displaystyle H_{0}^{i}$	$\displaystyle=\breve{P}_{i,0}^{-1}+A_{ii}^{\mathrm{T}}Q_{i}^{-1}A_{ii}$		(14a)
	$\displaystyle\breve{\sigma}^{i}_{0}$	$\displaystyle=\breve{x}_{0}^{i}+\breve{P}_{i,0}A_{ii}^{\mathrm{T}}(A_{ii}\breve{P}_{i,0}A_{ii}^{\mathrm{T}}+Q_{i})^{-1}(\hat{x}_{1}^{i}-A_{ii}\breve{x}_{0}^{i}-\sum_{l\in\mathbb{I}\setminus\{i\}}A_{il}\tilde{x}_{0}^{l})$		(14b)
	$\displaystyle\breve{\pi}^{i}_{0}$	$\displaystyle=\|\breve{x}_{0}^{i}-\breve{\sigma}^{i}_{0}\|^{2}_{\breve{P}_{i,0}^{-1}}+\|\hat{x}_{1}^{i}-A_{ii}\breve{\sigma}^{i}_{0}-\sum_{l\in\mathbb{I}\setminus\{i\}}A_{il}\tilde{x}_{0}^{l}\|^{2}_{Q_{i}^{-1}}$		(14c)

Let $\bar{x}_{1}^{i}:=A_{ii}\breve{x}_{0}^{i}+\sum_{l\in\mathbb{I}\setminus\{i\}}A_{il}\tilde{x}_{0}^{l}$ and $L_{i,0}=\breve{P}_{i,0}A_{ii}^{\mathrm{T}}(A_{ii}\breve{P}_{i,0}A_{ii}^{\mathrm{T}}+Q_{i})^{-1}$. $\breve{\pi}^{i}_{0}$ in (14c) can be reformulated as follows:

	$\displaystyle\breve{\pi}^{i}_{0}$	$\displaystyle=\|L_{i,0}(\hat{x}_{1}^{i}-\bar{x}_{1}^{i})\|^{2}_{\breve{P}_{i,0}^{-1}}+\|(I-A_{ii}L_{i,0})(\hat{x}_{1}^{i}-\bar{x}_{1}^{i})\|^{2}_{Q_{i}^{-1}}$
		$\displaystyle=\|\hat{x}_{1}^{i}-\bar{x}_{1}^{i}\|^{2}_{P_{i,1}^{-1}}$

where

	$\displaystyle P_{i,1}^{-1}$	$\displaystyle=L_{i,0}^{\mathrm{T}}\breve{P}_{i,0}^{-1}L_{i,0}+(I-A_{ii}L_{i,0})^{\mathrm{T}}Q_{i}^{-1}(I-A_{ii}L_{i,0})$
		$\displaystyle=L_{i,0}^{\mathrm{T}}(\breve{P}_{i,0}^{-1}+A_{ii}^{\mathrm{T}}Q_{i}^{-1}A_{ii})L_{i,0}-Q_{i}^{-1}A_{ii}L_{i,0}-L_{i,0}^{\mathrm{T}}A_{ii}^{\mathrm{T}}Q_{i}^{-1}+Q_{i}^{-1}$		(15)

According to Lemma 2, we have

$$L_{i,0}=(\breve{P}_{i,0}^{-1}+A_{ii}^{\mathrm{T}}Q_{i}^{-1}A_{ii})^{-1}A_{ii}^{\mathrm{T}}Q_{i}^{-1}$$

(16)

Then, the first term on the right-hand-side of (4.1) can be rewritten as:

		$\displaystyle\quad L_{i,0}^{\mathrm{T}}(\breve{P}_{i,0}^{-1}+A_{ii}^{\mathrm{T}}Q_{i}^{-1}A_{ii})L_{i,0}$
		$\displaystyle=Q_{i}^{-1}A_{ii}(\breve{P}_{i,0}^{-1}+A_{ii}^{\mathrm{T}}Q_{i}^{-1}A_{ii})^{-1}(\breve{P}_{i,0}^{-1}+A_{ii}^{\mathrm{T}}Q_{i}^{-1}A_{ii})(\breve{P}_{i,0}^{-1}+A_{ii}^{\mathrm{T}}Q_{i}^{-1}A_{ii})^{-1}A_{ii}^{\mathrm{T}}Q_{i}^{-1}$
		$\displaystyle=Q_{i}^{-1}A_{ii}(\breve{P}_{i,0}^{-1}+A_{ii}^{\mathrm{T}}Q_{i}^{-1}A_{ii})^{-1}A_{ii}^{\mathrm{T}}Q_{i}^{-1},$		(17)

Substituting (16) and (4.1) into (4.1) yields

	$\displaystyle P_{i,1}^{-1}$	$\displaystyle=Q_{i}-Q_{i}^{-1}A_{ii}(\breve{P}_{i,0}^{-1}+A_{ii}^{\mathrm{T}}Q_{i}^{-1}A_{ii})^{-1}A_{ii}^{\mathrm{T}}Q_{i}^{-1}$
		$\displaystyle=(Q_{i}+A_{ii}\breve{P}_{i,0}A_{ii}^{\mathrm{T}})^{-1}$		(18)

Therefore, it is further obtain $P_{i,1}=Q_{i}+A_{ii}\breve{P}_{i,0}A_{ii}^{\mathrm{T}}$. By minimizing $V^{i}_{1}$ in (4.1) with respect to $\hat{x}_{0}^{i}$, the arrival cost $V^{i,o}_{1}$ for the $i$th estimator can be derived as follows:

$$V_{1}^{i,o}=\min_{\hat{x}_{0}^{i}}V_{1}^{i}=\|\hat{x}_{1}^{i}-\bar{x}_{1}^{i}\|^{2}_{P_{i,1}^{-1}}$$

Next, let us proceed to the arrival cost $V_{2}^{i,o}$ by addressing the following optimization problem:

	$$V_{2}^{i,o}=\min_{\{\hat{x}_{j}^{i}\}_{j=0}^{j=1}}V_{2}^{i}~{}\text{subject to \eqref{eq:upd0_2}, \eqref{eq:upd0_4}, and\eqref{eq:upd0_3} for}~{}j=0,1$$			(19a)
where
	$\displaystyle V_{2}^{i}$	$\displaystyle=\|\hat{x}_{0}^{i}-\bar{x}_{0}^{i}\|^{2}_{P_{i,0}^{-1}}+\|\hat{v}_{0}^{[i]}\|^{2}_{R^{-1}}+\|\hat{w}_{0}^{i}\|^{2}_{Q_{i}^{-1}}+\|\hat{v}_{1}^{[i]}\|^{2}_{R^{-1}}+\|\hat{w}_{1}^{i}\|^{2}_{Q_{i}^{-1}}$		(19b)

By following the same procedure adopted to derive (4.1), $V_{2}^{i}$ can be expressed as:

$\displaystyle V_{2}^{i}$

$\displaystyle=\|\hat{x}_{0}^{i}-\breve{x}_{0}^{i}\|^{2}_{\breve{P}_{i,0}^{-1}}+\|\hat{v}_{1}^{[i]}\|^{2}_{R^{-1}}+\|\hat{w}_{0}^{i}\|^{2}_{Q_{i}^{-1}}+\|\hat{w}_{1}^{i}\|^{2}_{Q_{i}^{-1}},$

up to a constant term. Based on (8c), it is further obtained that

	$\displaystyle V_{2}^{i}$	$\displaystyle=\|\hat{x}_{0}^{i}-\breve{x}_{0}^{i}\|^{2}_{\breve{P}_{i,0}^{-1}}+\|y_{1}-C(A_{[:,i]}\hat{x}_{0}^{i}+\sum_{l\in\mathbb{I}\setminus\{i\}}A_{[:,l]}\tilde{x}_{0}^{l})\|^{2}_{R^{-1}}+\|\hat{w}_{0}^{i}\|^{2}_{Q_{i}^{-1}}+\|\hat{w}_{1}^{i}\|^{2}_{Q_{i}^{-1}}$
		$\displaystyle=\|\hat{x}_{0}^{i}-\check{x}_{0}^{i}\|^{2}_{\check{P}_{i,0}^{-1}}+\check{\pi}_{0}^{i}+\|\hat{w}_{0}^{i}\|^{2}_{Q_{i}^{-1}}+\|\hat{w}_{1}^{i}\|^{2}_{Q_{i}^{-1}}$

where

	$\displaystyle\check{P}_{i,0}$	$\displaystyle=\breve{P}_{i,0}-\breve{P}_{i,0}A_{[:,i]}^{\mathrm{T}}C^{\mathrm{T}}(CA_{[:,i]}\breve{P}_{i,0}A_{[:,i]}^{\mathrm{T}}C^{\mathrm{T}}+R)^{-1}CA_{[:,i]}\breve{P}_{i,0}$
	$\displaystyle\check{x}_{0}^{i}$	$\displaystyle=\breve{x}_{0}^{i}+\breve{P}_{i,0}A_{[:,i]}^{\mathrm{T}}C^{\mathrm{T}}(CA_{[:,i]}\breve{P}_{i,0}A_{[:,i]}^{\mathrm{T}}C^{\mathrm{T}}+R)^{-1}(y_{1}-C(A_{[:,i]}\breve{x}_{0}^{i}+\sum_{l\in\mathbb{I}\setminus\{i\}}A_{[:,l]}\tilde{x}_{0}^{l}))$
	$\displaystyle\check{\pi}_{0}^{i}$	$\displaystyle=\|\check{x}_{0}^{i}-\breve{x}_{0}^{i}\|^{2}_{\breve{P}_{i,0}^{-1}}+\|y_{1}-C(A_{[:,i]}\check{x}_{0}^{i}+\sum_{l\in\mathbb{I}\setminus\{i\}}A_{[:,l]}\tilde{x}_{0}^{l})\|^{2}_{R^{-1}}$

$\check{\pi}_{0}^{i}$ is a constant and is neclected in deriving $V_{2}^{i}$. Then, by applying the same procedure used to obtain (4.1) and (4.1), it is derived that:

	$\displaystyle V_{2}^{i}$	$\displaystyle=\|\hat{x}_{0}^{i}-\check{x}_{0}^{i}\|^{2}_{\check{P}_{i,0}^{-1}}+\|\hat{x}_{1}^{i}-A_{ii}\hat{x}_{0}^{i}-\sum_{l\in\mathbb{I}\setminus\{i\}}A_{il}\tilde{x}_{0}^{l}\|^{2}_{Q_{i}^{-1}}+\|\hat{w}_{1}^{i}\|^{2}_{Q_{i}^{-1}}$
		$\displaystyle=\|\hat{x}_{0}^{i}-\bar{\sigma}_{0}^{i}\|^{2}_{\bar{H}_{0}^{i}}+\bar{\pi}_{0}^{i}+\|\hat{w}_{1}^{i}\|^{2}_{Q_{i}^{-1}}$		(20)

where

	$\displaystyle\bar{H}_{0}^{i}$	$\displaystyle=\check{P}_{i,0}^{-1}+A_{ii}^{\mathrm{T}}Q_{i}^{-1}A_{ii}$
	$\displaystyle\bar{\sigma}^{i}_{0}$	$\displaystyle=\check{x}_{0}^{i}+\check{P}_{i,0}A_{ii}^{\mathrm{T}}(A_{ii}\check{P}_{i,0}A_{ii}^{\mathrm{T}}+Q_{i})^{-1}(\hat{x}_{1}^{i}-A_{ii}\check{x}_{0}^{i}-\sum_{l\in\mathbb{I}\setminus\{i\}}A_{il}\tilde{x}_{0}^{l})$
	$\displaystyle\breve{P}_{i,1}$	$\displaystyle=Q_{i}+A_{ii}\check{P}_{i,0}A_{ii}^{\mathrm{T}}$
	$\displaystyle\bar{\pi}^{i}_{0}$	$\displaystyle=\|\hat{x}_{1}^{i}-A_{ii}\check{x}_{0}^{i}-\sum_{l\in\mathbb{I}\setminus\{i\}}A_{il}\tilde{x}_{0}^{l}\|^{2}_{\breve{P}_{i,1}^{-1}}$

Let us further define $\breve{x}_{1}^{i}:=A_{ii}\check{x}_{0}^{i}+\sum_{l\in\mathbb{I}\setminus\{i\}}A_{il}\tilde{x}_{0}^{l}$. Based on (4.1), we apply the forward dynamic programming method to the optimization problem (19), and it is obtained that

$\displaystyle V_{2}^{i,o}$

$\displaystyle=\min_{\hat{x}_{1}^{i}}\big{\{}\min_{\hat{x}_{0}^{i}}\{\|\hat{x}_{0}^{i}-\bar{\sigma}_{0}^{i}\|^{2}_{\bar{H}_{0}^{i}}\}+\|\hat{x}_{1}^{i}-\breve{x}_{1}^{i}\|^{2}_{\breve{P}_{i,1}^{-1}}+\|\hat{w}_{1}^{i}\|^{2}_{Q_{i}^{-1}}\big{\}}~{}\text{subject to \eqref{eq:upd0_2} for}~{}j=1$

(21)

It is noted that the optimal value of $\hat{x}_{0}^{i}$ for optimization problem (21) is $\bar{\sigma}_{0}^{i}$. Therefore, it is further derived

$\displaystyle V_{2}^{i,o}$

$\displaystyle=\min_{\hat{x}_{1}^{i}}\big{\{}\|\hat{x}_{1}^{i}-\breve{x}_{1}^{i}\|^{2}_{\breve{P}_{i,1}^{-1}}+\|\hat{w}_{1}^{i}\|^{2}_{Q_{i}^{-1}}\big{\}}~{}\text{subject to \eqref{eq:upd0_2} for}~{}j=1$

(22)

In the following, according to (8a), the objective function of (22) can be rewritten as

	$\displaystyle V_{2}^{i}$	$\displaystyle=\|\hat{x}_{1}^{i}-\breve{x}_{1}^{i}\|^{2}_{\breve{P}_{i,1}^{-1}}+\|\hat{x}_{2}^{i}-A_{ii}\hat{x}_{1}^{i}-\sum_{l\in\mathbb{I}\setminus\{i\}}A_{il}\tilde{x}_{1}^{l}\|^{2}_{Q_{i}^{-1}}$
		$\displaystyle=\|\hat{x}_{1}^{i}-\breve{\sigma}_{1}^{i}\|^{2}_{H_{1}^{i}}+\breve{\pi}_{1}^{i}$		(23)

where

	$\displaystyle H_{1}^{i}$	$\displaystyle=\breve{P}_{i,1}^{-1}+A_{ii}^{\mathrm{T}}Q_{i}^{-1}A_{ii}$
	$\displaystyle\breve{\sigma}^{i}_{1}$	$\displaystyle=\breve{x}_{1}^{i}+\breve{P}_{i,1}A_{ii}^{\mathrm{T}}(A_{ii}\breve{P}_{i,1}A_{ii}^{\mathrm{T}}+Q_{i})^{-1}(\hat{x}_{2}^{i}-A_{ii}\breve{x}_{1}^{i}-\sum_{l\in\mathbb{I}\setminus\{i\}}A_{il}\tilde{x}_{1}^{l})$
	$\displaystyle P_{i,2}$	$\displaystyle=Q_{i}+A_{ii}\breve{P}_{i,1}A_{ii}^{\mathrm{T}}$
	$\displaystyle\breve{\pi}^{i}_{1}$	$\displaystyle=\|\hat{x}_{2}^{i}-A_{ii}\breve{x}_{1}^{i}-\sum_{l\in\mathbb{I}\setminus\{i\}}A_{il}\tilde{x}_{1}^{l}\|^{2}_{P_{i,2}^{-1}}$

Let $\bar{x}_{2}^{i}:=A_{ii}\breve{x}_{1}^{i}+\sum_{l\in\mathbb{I}\setminus\{i\}}A_{il}\tilde{x}_{1}^{l}$. By minimizing $V_{2}^{i}$ in (4.1) with respect to $\hat{x}_{1}^{i}$, the arrival cost $V_{2}^{i,o}$ in (22) of the $i$th estimator can be obtained as:

$$V_{2}^{i,o}=\min_{\hat{x}_{1}^{i}}V_{2}^{i}=\|\hat{x}_{2}^{i}-\bar{x}_{2}^{i}\|^{2}_{P_{i,2}^{-1}}$$