*3.3. Robust H*∞ *Filter*

The aim of the *H*∞ filter is to obtain the state estimation minimizing the following cost function [34].

$$J\_{\infty} = \frac{\sum\_{k=0}^{N-1} ||\mathbf{x}\_k - \hat{\mathbf{x}}\_k||\_{M\_k}^2}{||\mathbf{x}\_0 - \hat{\mathbf{x}}\_0||\_{\mathbf{Y}\_0}^2 + \sum\_{k=0}^{N-1} \left( ||\mathbf{w}\_k||\_{Q\_k}^2 + ||\mathbf{u}\_k||\_{R\_k}^2 \right)} \tag{42}$$

Based on game theory, the design of the *H*∞ filter is a game between the filter designer and nature. Nature's goal is to maximize the estimation error by properly choosing *x*0, *wk*, and *nk*. The cost function is defined in a fractional form to create a fair game by preventing nature using extremely large noises.

The above optimization problem is difficult to solve directly. An alternative strategy is to choose an upper bound and estimate the state satisfying the following constraint.

$$J\_{\infty} < \gamma^2 \tag{43}$$

where *γ* is the performance bound to be designed. According to the above equation, a new cost function is defined as follows:

$$J = -\frac{1}{\theta} \|\mathbf{x}\_0 - \mathbf{\hat{x}}\_0\|\_{Y\_0}^2 + \sum\_{k=0}^{N-1} \left( \|\mathbf{x}\_k - \mathbf{\hat{x}}\_k\|\_{M\_k}^2 - \frac{1}{\theta} \left( \|w\_k\|\_{Q\_k}^2 + \|n\_k\|\_{R\_k}^2 \right) \right) < 1\tag{44}$$

Therefore, the original optimization problem is transformed into the following minimax problem.

$$J^\* = \min\_{\p\_k} \max\_{\mathcal{X}\_0, \mathcal{W}\_k, \mathcal{R}\_k} J \tag{45}$$

Yang [29] solved the minimax problem and proposed an extended *H*∞ filter (E*H*∞F) for the nonlinear system, which was similar to EKF in form.

*Appl. Sci.* **2019**, *9*, 56

$$\begin{cases} \mathbf{x}\_{k|k-1} = f(\mathbf{x}\_{k-1|k-1})\\ \mathbf{P}\_{k|k-1} = \nabla f \mathbf{P}\_{k-1|k-1} \nabla f^T + \mathbf{Q} \end{cases} \tag{46}$$

$$\begin{aligned} \mathbf{K}\_{k} &= \mathbf{P}\_{k|k-1} \nabla h^{\mathrm{T}} \left( \nabla h \mathbf{P}\_{k|k-1} \nabla h^{\mathrm{T}} + \mathbf{R}\_{k} \right)^{-1} \\ \mathbf{x}\_{k|k} &= \mathbf{x}\_{k|k-1} + \mathbf{K}\_{k} \left( \mathbf{z}\_{k} - h \left( \mathbf{x}\_{k-1|k-1} \right) \right) \\ \mathbf{P}\_{k|k}^{-1} &= \mathbf{P}\_{k|k-1}^{-1} + \nabla h^{\mathrm{T}} \mathbf{R}\_{k}^{-1} \nabla h - \gamma^{-2} \mathbf{Z}\_{n} \end{aligned} \tag{47}$$

where *I<sup>n</sup>* is the *n*-dimension identity matrix. The E*H*∞F is a Jacobian-based filter, and can be extended with cubature rules for higher precision in nonlinear systems as Chandra did in [30]. The time-update phase of the cubature *H*∞ filter (C*H*∞F) is the same as the CKF and expressed as the equation. Therefore, we only provided the measurement-update equations as follows:

$$\begin{aligned} \mathbf{P\_{xz}} &= H\_k \mathbf{P\_{k|k-1}} H\_k^T + \mathbf{R}\_k\\ \mathbf{K\_k} &= \mathbf{P\_{xx}} \mathbf{P\_{zz}^{-1}}\\ \hat{\mathbf{x}}\_{k|k} &= \hat{\mathbf{x}}\_{k|k-1} + \mathbf{K\_k} \left( \mathbf{z}\_k - \hbar \left( \hat{\mathbf{x}}\_{k|k-1} \right) \right) \\ \mathbf{P\_{k|k}^{-1}} &= \mathbf{P\_{k|k-1}^{-1}} + H\_k^T \mathbf{R}\_k^{-1} H\_k - \gamma^{-2} \mathbf{Z\_n} \end{aligned} \tag{48}$$

where the cross-covariance matrix *P*xz and the measurement matrix *H<sup>k</sup>* are evaluated using a cubature rule. It can be seen from Equations (47) and (48) that the main difference between the E*H*∞F and the Kalman filter was the formula to update the covariance matrix *Pk*|*k*. In this way, the E*H*∞F acts to reduce the confidence of the state estimation in case the unexpected noises negatively impact the performance. Applying the information-description of the Kalman filter on the above C*H*∞F, a cubature *H*∞ information filter (C*H*∞IF) can be formulated as follows:

$$\begin{aligned} \mathbf{P}\_{\mathbf{zz}} &= H\_k \mathbf{P}\_{k|k-1} H\_k^\mathrm{T} + \mathbf{R}\_k\\ \dot{\mathbf{z}}\_k &= H\_k^\mathrm{T} \mathbf{R}\_k^{-1} \left( \mathbf{z}\_k - h \left( \hat{\mathbf{x}}\_{k|k-1} \right) + H\_k \hat{\mathbf{x}}\_{k|k-1} \right) - \gamma^{-2} \left( \hat{\mathbf{x}}\_{k|k-1} + \mathbf{P}\_{\mathbf{xx}} P\_{\mathbf{zz}}^{-1} \left( \mathbf{z}\_k - h \left( \hat{\mathbf{x}}\_{k|k-1} \right) \right) \right) \\ \dot{\mathbf{y}}\_{k|k} &= \hat{\mathbf{y}}\_{k|k-1} + \dot{\mathbf{z}}\_k\\ \mathbf{Y}\_{k|k} &= \mathbf{Y}\_{k|k-1} + \mathbf{H}\_k^\mathrm{T} \mathbf{R}\_k^{-1} \mathbf{H}\_k - \gamma^{-2} \mathbf{Z}\_n \end{aligned} \tag{49}$$

where the covariance matrix of measurement error vector *P*zz is an *m*-dimension matrix. As previously stated, *m* is generally large in direct VO, which means that the inversion of *P*zz in the above equation is not practical. We used the expressions of the gain matrix in the Kalman filter to simplify the inversion. In this way, the measurement-update phase in C*H*∞IF was divided into two steps as follows:

(1) Evaluate the measurement matrix *H<sup>k</sup>* with a cubature rule and update the information matrix using the information filter.

$$\Upsilon\_k' = \Upsilon\_{k|k-1} + H\_k^T \mathcal{R}\_k^{-1} H\_k \tag{50}$$

(2) Evaluate the gain matrix *K*∞ and execute the measurement update of the *H*∞ filter.

$$\begin{aligned} \mathbf{K}\_{\infty} &= \mathbf{Y}\_{k}^{\prime \prime - 1} \mathbf{H}\_{k}^{\mathrm{T}} \mathbf{R}\_{k}^{-1} \\ \dot{\mathbf{z}}\_{k} &= \mathbf{H}\_{k}^{\mathrm{T}} \mathbf{R}\_{k}^{-1} \left( \mathbf{z}\_{k} - \hbar \left( \hat{\mathbf{x}}\_{k|k-1} \right) + \mathbf{H}\_{k} \hat{\mathbf{x}}\_{k|k-1} \right) - \gamma^{-2} \left( \hat{\mathbf{x}}\_{k|k-1} + \mathbf{K}\_{\infty} \left( \mathbf{z}\_{k} - \hbar \left( \hat{\mathbf{x}}\_{k|k-1} \right) \right) \right) \\ \dot{\mathbf{y}}\_{k|k} &= \hat{\mathbf{y}}\_{k|k-1} + \dot{\mathbf{z}}\_{k} \\ \mathbf{Y}\_{k|k} &= \mathbf{Y}\_{k}^{\prime} - \gamma^{-2} \mathbf{Z}\_{n} \end{aligned} \tag{51}$$

It is easy to prove that the above two-step measurement-update process is equal to Equation (49). The inversion of the *m*-dimension matrix is replaced by the inversion of a 9-dimensional matrix *Y<sup>k</sup>* .

It is worth noting that the measurement-update phase of the proposed C*H*∞IF was different to the one with the same abbreviation that Chandra derived in [31]. Compared with Equation (49), Chandra's method omitted the second term of *i<sup>k</sup>* when using the technique to analyze the estimation

error in [35,36]. The error transition function of the measurement-update phase of our method is as follows:

$$
\tilde{\mathbf{x}}\_{k|k} = \tilde{\mathbf{x}}\_{k|k-1} - \mathbf{K}\_k \tilde{\mathbf{z}}\_k \tag{52}
$$

$$\mathbf{K}\_k = P\_{k|k} \left( H\_k^\mathrm{T} \mathbf{R}\_k^{-1} - \gamma^{-2} \mathbf{K}\_\infty \right) \tag{53}$$

where *<sup>~</sup> z<sup>k</sup>* is the predicted error of the measurement *zk*, and formulated as follows:

$$
\tilde{\mathbf{z}}\_k = \mathbf{z}\_k - h \left( \mathbf{\hat{x}}\_{k|k-1} \right) \tag{54}
$$

While the error transition function of the measurement-update phase of Chandra's method is related to the predicted mean value of the state vector ˆ*xk*|*k*−<sup>1</sup> as follows:

$$\widetilde{\mathbf{x}}\_{k|k} = \widetilde{\mathbf{x}}\_{k|k-1} - \mathbf{P}\_{k|k} \mathbf{H}\_k^\mathsf{T} \mathbf{R}\_k^{-1} \widetilde{\mathbf{z}}\_k - \gamma^{-2} \mathbf{P}\_{k|k} \mathbf{\hat{x}}\_{k|k-1} \tag{55}$$

This goes against the convergence of the filter. In contrast, the error transition function of our method is in the form of a weighted error function as used in [35,36], which makes a convergent filter.
