## 3 Switched Variational Estimation Filtering in Underwater Navigation
### Proposed Optimal Variational Estimation in Navigation Information Acquisition
 \tag{24}\]  
**Case 1.1:** In iteration \(l\), the expectation of \(\Big{\{}\{\mathbf{Z}(t_{k})-h[\mathbf{X}(t_{k})]\}\{\mathbf{Z}(t_{k})-h[ \mathbf{X}(t_{k})]\}^{T}\Big{\}}\) is calculated as:\[ \begin{array}{l}\mathrm{E}^{(l)}\Big{\{}\{\mathbf{Z}(t_{k})-h[\mathbf{X}(t_{k})] \}\big{\{}\mathbf{Z}(t_{k})-h[\mathbf{X}(t_{k})]\}^{T}\Big{\}}\\ =\mathrm{E}^{(l)}\Big{\{}\mathbf{Z}(t_{k})-h\Big{[}\mathbf{X}^{(l)}(t_{k}) \Big{]}+h\Big{[}\mathbf{X}^{(l)}(t_{k})\Big{]}-h[\mathbf{X}(t_{k})]\Big{\}} \ \times\Big{\{}\mathbf{Z}(t_{k})-h\Big{[}\mathbf{X}^{(l)}(t_{k})\Big{]}+h \Big{[}\mathbf{X}^{(l)}(t_{k})\Big{]}-h[\mathbf{X}(t_{k})]\Big{\}}^{T}\\ =\Big{\{}\mathbf{Z}(t_{k})-h\Big{[}\mathbf{X}^{(l)}(t_{k})\Big{]}\Big{\}} \Big{\{}\mathbf{Z}(t_{k})-h\Big{[}\mathbf{X}^{(l)}(t_{k})\Big{]}\Big{\}}^{T}+ \mathbf{H}_{\mathbf{k}}\mathbf{P}^{(l)}\big{(}t_{k}^{+}\big{)}\mathbf{H}_{k}^ {T}\end{array}  \tag{25}\]  
**Case 2.1:** The expectation of \(\Big{\{}\{\mathbf{Z}(t_{k})-h[\mathbf{X}(t_{k})]\}\{\mathbf{Z}(t_{k})-h[ \mathbf{X}(t_{k})]\}^{T}\Big{\}}\) in iteration \(l\) is estimated by using the cubature rule. The cubature points can be generated as:  
\[ \begin{array}{l}\chi_{ik}^{s(l)}=\mathbf{X}^{(l)}(t_{k})+\mathbf{C}_{i}^{s},\quad i=1,2,\ldots,2n_{x}\end{array}  \tag{26}\]  
where \(\mathbf{C}_{i}^{s}=\sqrt{n_{x}\mathbf{P}^{(l)}(t_{k})}[\mathbf{I}_{x}\quad- \mathbf{I}_{x}]_{i}\); \(\mathbf{I}_{x}\) is the \(n_{x}\)-order identity matrix; \([\bullet]_{i}\) represents column \(i\) of the matrix. Then, the nonlinear integral, which is the expectation of  
\[\Big{\{}\{\mathbf{Z}(t_{k})-h[\mathbf{X}(t_{k})]\}\{\mathbf{Z}(t_{k})-h[ \mathbf{X}(t_{k})]\}^{T}\Big{\}}\Big{\}}\] in iteration \(l\), is calculated as:  
\[ \begin{array}{l}\mathrm{E}^{(l)}\Big{\{}\{\mathbf{Z}(t_{k})-h[\mathbf{X}(t_ {k})]\}\{\mathbf{Z}(t_{k})-h[\mathbf{X}(t_{k})]\}^{T}\Big{\}}=\frac{1}{2n_{x} }\!\sum_{i=1}^{2n_{x}}\!\Big{[}\!\mathbf{Z}_{k}^{*(l)}-h\Big{(}\!\chi_{i,k}^{*( l)}\Big{)}\Big{]}\Big{[}\!\mathbf{Z}_{k}^{*(l)}-h\Big{(}\!\chi_{i,k}^{*(l)}\Big{)} \Big{]}^{T}\end{array}  \tag{27}\]  
where \(\mathbf{Z}_{k}^{*(l)}\) is calculated by using Equation (28). \[\mathbf{Z}_{k}^{*(l)}=\frac{1}{2n_{x}}\!\sum_{i=1}^{2n_{x}}h\Big{(}\!\chi_{i,k }^{*(l)}\Big{)} \tag{28}\]  
When the measurement information is lost, \(\mathrm{E}^{(l)}\big{[}\mathbf{Z}(t_{k})\mathbf{Z}^{T}(t_{k})\big{]}\) is derived as:  
\[\mathrm{E}^{(l)}\Big{[}\mathbf{Z}(t_{k})\mathbf{Z}^{T}(t_{k})\Big{]}=\mathbf{ R}_{k}^{(l)} \tag{29}\]  
**Remark 3**. : _In iteration \(l+1\), the update of DOF and the inverse scale matrix of \(\mathbf{R}_{k}\) can be expressed as:_  
\[\left\{ \begin{array}{l}u^{(l+1)}(t_{k})=u^{(l)}\big{(}t_{k}^{+}\big{)}+1\\ \mathbf{U}^{(l+1)}(t_{k})=\mathbf{B}_{k}^{(l)}+\mathbf{U}^{(l)}\big{(}t_{k}^{+ }\big{)}\end{array} \right. \tag{30}\]  
_The inverse of \(\mathbf{R}_{k}\) and inverse of \(\mathbf{P}\big{(}t_{k}^{+}\big{)}\) can be determined by:_  
\[\left\{ \begin{array}{l}\mathrm{E}^{(l+1)}\Big{(}\mathbf{R}_{k}^{-1}\Big{)}= \mathrm{E}^{(l)}[\xi(t_{k})]\Big{[}u^{(l+1)}(t_{k})-n_{Z}-1\Big{]}\mathbf{U}^{ -(l+1)}(t_{k})\\ \mathrm{E}^{(l+1)}\big{[}\mathbf{P}^{-1}\big{(}t_{k}^{+}\big{)}\big{]}=\Big{[} t^{(l+1)}(t_{k})-n_{x}-1\Big{]}\mathbf{T}^{-(l+1)}(t_{k})\end{array} \right. \tag{31}\]  
_The expectations of \(\mathbf{R}_{k}\) and \(\mathbf{P}\big{(}t_{k}^{+}\big{)}\) in iteration \(l+1\) are expressed as:_  
\[\left\{ \begin{array}{l}\mathrm{E}^{(l+1)}(\mathbf{R}_{k})=\Big{[}\mathrm{E} ^{(l+1)}\Big{(}\mathbf{R}_{k}^{-1}\Big{)}\Big{]}^{-1}\\ \mathrm{E}^{(l+1)}\big{[}\mathbf{P}\big{(}t_{k}^{+}\big{)}\big{]}=\Big{\{} \mathrm{E}^{(l+1)}\big{[}\mathbf{P}^{-1}\big{(}t_{k}^{+}\big{)}\big{]}\Big{\}}^{-1} \end{array} \right. \tag{32}\]  
2. The update of state vector and error covariance matrix. Set \(\kappa=\mathbf{X}(t_{k})\); then, the logarithm of \(\mathrm{q}(\kappa)\) is calculated in Equation (33) in iteration \(l+1\)\[