*3.1. Bayes Filter for Nonlinear Visual–Inertial Navigation System*

Consider the following nonlinear system

$$\begin{aligned} \mathbf{x}\_k &= f(\mathbf{x}\_{k-1}) + \mathbf{w}\_{k-1} \\ \mathbf{z}\_k &= h(\mathbf{x}\_k) + \mathbf{u}\_k \end{aligned} \tag{15}$$

where *f*(·) and *h*(·) are arbitrary nonlinear functions and *wk*−<sup>1</sup> and *n<sup>k</sup>* are the process noise and the measurement noise, respectively. Under the Gaussian assumption of noises, the Bayes filter for nonlinear system described by the information vector and information matrix was formulated as an iterated process of a time-update phase and a measurement-update phase.

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

(1) Time-update phase

$$\begin{cases}
\hat{\mathbf{x}}\_{k|k-1} = \int\_{\mathbb{R}^n} f(\mathbf{x}) \mathcal{M}\Big(\mathbf{x}; \hat{\mathbf{x}}\_{k-1|k-1}, P\_{k-1|k-1}\Big) d\mathbf{x} \\
\mathbf{P}\_{k|k-1} = \int\_{\mathbb{R}^n} f(\mathbf{x}) f^\top(\mathbf{x}) \mathcal{M}\Big(\mathbf{x}; \hat{\mathbf{x}}\_{k-1|k-1}, P\_{k-1|k-1}\Big) d\mathbf{x} - \hat{\mathbf{x}}\_{k|k-1} \hat{\mathbf{x}}\_{k|k-1}^\top \\
\mathbf{Y}\_{k|k-1} = \mathbf{P}\_{k|k-1}^{-1} \\
y\_{k|k-1} = \mathbf{Y}\_{k|k-1} \hat{\mathbf{x}}\_{k|k-1}
\end{cases} \tag{16}$$

(2) Measurement-update phase

$$\begin{aligned} \dot{\mathbf{z}}\_{k|k-1} &= \int\_{\mathbb{R}^n} h(\mathbf{x}) \mathcal{N} \Big( \mathbf{x}; \hat{\mathbf{x}}\_{k|k-1}, P\_{k|k-1} \Big) d\mathbf{x} \\ \mathbf{P}\_{xx} &= \int\_{\mathbb{R}^n} \mathbf{x} h^T(\mathbf{x}) \mathcal{N} \Big( \mathbf{x}; \hat{\mathbf{x}}\_{k|k-1}, P\_{k|k-1} \Big) d\mathbf{x} - \hat{\mathbf{x}}\_{k|k-1} \hat{\mathbf{z}}\_{k|k-1}^T \\ H\_k &= \left( \mathbf{Y}\_{k|k-1} \mathbf{P}\_{\mathbf{x}\mathbf{z}} \right)^T \\ \hat{\mathbf{y}}\_{k|k} &= \hat{\mathbf{y}}\_{k|k-1} + H\_k^T \mathbf{R}\_k^{-1} \Big( \mathbf{z}\_k - \hat{\mathbf{x}}\_{k|k-1} \Big) + H\_k \hat{\mathbf{x}}\_{k|k-1} \\ \mathbf{Y}\_{k|k} &= \mathbf{Y}\_{k|k-1} + H\_k^T \mathbf{R}\_k^{-1} H\_k \end{aligned} \tag{17}$$

The state estimation could be recovered from the information state

$$\begin{aligned} P\_{k|k} &= \Upsilon\_{k|k}^{-1} \\ \mathfrak{X}\_{k|k} &= P\_{k|k} \mathfrak{Y}\_{k|k} \end{aligned} \tag{18}$$

According to the measurement model in Equation (14), only a part of the state vector appeared explicitly in the measurement equation. We defined the state vector explicitly shown in the measurement model as follows:

$$\mathbf{x}\_{1} = \begin{bmatrix} \mathbf{\stackrel{I}{\mathbf{s}}} \mathbf{\stackrel{\mathbf{s}}{\mathbf{p}}}\\\mathbf{\stackrel{\mathbf{s}}{\mathbf{p}}} \mathbf{\stackrel{\mathbf{s}}{\mathbf{p}}} \end{bmatrix} \in \mathbb{R}^{n\_{1}} \tag{19}$$

where *n*<sup>1</sup> = 6. The rest state vector is

$$\mathfrak{w}\_2 = {}^{\mathrm{SI}}\mathfrak{v} \in \mathbb{R}^{n\_2}.\tag{20}$$

Correspondingly, the covariance matrix *P* of the state vector is composed of four blocks as follows:

$$P = \begin{bmatrix} \mathbf{P}\_{11} & \mathbf{P}\_{12} \\ \mathbf{P}\_{21} & \mathbf{P}\_{22} \end{bmatrix} \tag{21}$$

Applying the above partition of the state vector to the evaluation of the predicted measurements (the first equation in Equation (17)), one can obtain.

$$\begin{split} \mathfrak{S}\_{k|k-1} &= \int\_{\mathbb{R}^n} h(\mathbf{x}) \mathcal{N} \Big( \mathbf{x}; \mathbf{\hat{x}}\_{k|k-1}, P\_{k-1|k-1} \Big) d\mathbf{x} \\ &= \int\_{\mathbb{R}^{n1}} h(\mathbf{x}) \mathcal{N} \Big( \mathbf{x}\_1; \mathbf{\hat{x}}\_{1,k|k-1}, P\_{11,k|k-1} \Big) d\mathbf{x}\_1 \end{split} \tag{22}$$

As shown by Equation (22), the original 9-dimensional integral is simplified to a 6-dimensional integral. The evaluation of the cross-covariance matrix (the second equation in Equation (17)) can also be simplified in a similar way.

$$\begin{split} P\_{xz} &= \int\_{\mathbb{R}^n} x h^\mathrm{T}(x) \mathcal{N} \left( x; \mathbf{\hat{x}}\_{k-1|k-1}, P\_{k-1|k-1} \right) dx - \mathbf{\hat{x}}\_{k|k-1} \mathbf{\hat{z}}\_{k|k-1}^\mathrm{T} \\ &= \int\_{\mathbb{R}^{n\_1}} \int\_{\mathbb{R}^{n\_2}} \left[ \begin{array}{c} \mathbf{x}\_1 \\\\ \mathbf{x}\_2 \end{array} \right] h^\mathrm{T}(\mathbf{x}\_1) p(\mathbf{x}\_1 \mathbf{x}\_2) d\mathbf{x}\_2 d\mathbf{x}\_1 - \mathbf{\hat{x}}\_{k|k-1} \mathbf{\hat{z}}\_{k|k-1}^\mathrm{T} \end{split} \tag{23}$$

where *p*(*x*1, *x*2) is the joint probability density function of two random vectors *x*<sup>1</sup> and *x*<sup>2</sup> i.e., the probability density function of the Gaussian distribution. Based on the formula of conditional probability, one can obtain

$$p(\mathbf{x}\_1, \mathbf{x}\_2) = p(\mathbf{x}\_2 | \mathbf{x}\_1) p(\mathbf{x}\_1) \tag{24}$$

Substituting Equation (24) into Equation (23) yields

$$\mathbf{P}\_{\mathbf{X}\overline{\mathbf{z}}} = \int\_{\mathbb{R}^{n\_1}} \int\_{\mathbb{R}^{n\_2}} \left[ \begin{array}{c} \mathbf{x}\_1 \\ \mathbf{x}\_2 \end{array} \right] h^\mathbf{T}(\mathbf{x}\_1) p(\mathbf{x}\_2|\mathbf{x}\_1) d\mathbf{x}\_2 p(\mathbf{x}\_1) d\mathbf{x}\_1 - \mathbf{\hat{x}}\_{k|k-1} \mathbf{\hat{z}}\_{k|k-1}^\mathbf{T} \tag{25}$$

With the Gaussian assumption of the state vector, the distribution of *x*<sup>2</sup> conditional on *x*<sup>1</sup> is a multivariate Gaussian with the mean value of *x*ˆ2 + *P*21*P*−<sup>1</sup> <sup>11</sup> (*x*<sup>1</sup> − *x*ˆ1) and the covariance matrix of *<sup>P</sup>*<sup>22</sup> <sup>−</sup> *<sup>P</sup>*21*P*−<sup>1</sup> <sup>11</sup> *P*12. Thus, the inner integral in Equation (25) is solved as

$$\int\_{\mathbb{R}^{n\_2}} \begin{bmatrix} \mathbf{x}\_1 \\ \mathbf{x}\_2 \end{bmatrix} h^\mathrm{T}(\mathbf{x}\_1) p(\mathbf{x}\_2|\mathbf{x}\_1) d\mathbf{x}\_2 = \begin{bmatrix} \mathbf{x}\_1 \\ \mathbf{\hat{x}}\_2 + \mathbf{P}\_{21} \mathbf{P}\_{11}^{-1} (\mathbf{x}\_1 - \mathbf{\hat{x}}\_1) \end{bmatrix} h^\mathrm{T}(\mathbf{x}\_1). \tag{26}$$

The cross-covariance matrix is evaluated as follows:

*Pxz* = R*n*1 *<sup>x</sup>*<sup>1</sup> *<sup>x</sup>*ˆ2,*<sup>k</sup>*|*k*−<sup>1</sup> <sup>+</sup> *<sup>P</sup>*21,*k*|*k*−1*P*−<sup>1</sup> 11,*k*|*k*−1 *x*<sup>1</sup> − *x*ˆ1,*<sup>k</sup>*|*k*−<sup>1</sup> *<sup>h</sup>*T(*x*1)*p*(*x*1)*dx*<sup>1</sup> <sup>−</sup> *<sup>x</sup>*<sup>ˆ</sup> *<sup>k</sup>*|*k*−1*z*ˆ<sup>T</sup> *k*|*k*−1 = <sup>R</sup>*n*<sup>1</sup> *x*1*h*T(*x*1)*p*(*x*1)*dx*<sup>1</sup> R*n*1 *<sup>x</sup>*ˆ2,*<sup>k</sup>*|*k*−<sup>1</sup> <sup>+</sup> *<sup>P</sup>*21,*k*|*k*−1*P*−<sup>1</sup> 11,*k*|*k*−1 *x*<sup>1</sup> − *x*ˆ1,*<sup>k</sup>*|*k*−<sup>1</sup> *h*T(*x*1)*p*(*x*1)*dx*<sup>1</sup> <sup>−</sup> *<sup>x</sup>*<sup>ˆ</sup> *<sup>k</sup>*|*k*−1*z*ˆ<sup>T</sup> *k*|*k*−1 = <sup>R</sup>*n*<sup>1</sup> *<sup>x</sup>*1*h*T(*x*1)*p*(*x*1)*dx*<sup>1</sup> <sup>−</sup> **<sup>x</sup>**ˆ1,*<sup>k</sup>*|*k*−1**z**ˆ<sup>T</sup> *k*|*k*−1 *<sup>P</sup>*21,*k*|*k*−1*P*−<sup>1</sup> 11,*k*|*k*−1 <sup>R</sup>*n*<sup>1</sup> *<sup>x</sup>*1*h*T(*x*1)*p*(*x*1)*dx*<sup>1</sup> <sup>−</sup> *<sup>x</sup>*ˆ1,*<sup>k</sup>*|*k*−1*z*ˆ<sup>T</sup> *k*|*k*−1 (27)

In this way, the integrals in 9-dimensional space are simplified into integrals in 6-dimensional space. This is practically useful when applying high-degree cubature rules and will be discussed later.
