## 3 Switched Variational Estimation Filtering in Underwater Navigation
### Proposed Optimal Variational Estimation in Navigation Information Acquisition  
\(\mathrm{Pro}[\Theta|\mathbf{Z}_{1:k}]\) is used to estimate the optimal solution of \(\Theta\). However, the joint posteriori PDF cannot be solved, so the variational Bayesian algorithm is introduced to search for an optimal PDF \(\mathrm{q}(\bullet)\) to characterize the joint posteriori PDF as:  
\[\mathrm{Pro}[\Theta|\mathbf{Z}_{1:k}]\approx\mathrm{q}[\mathbf{X}(t_{k})] \mathrm{q}\big{[}\mathbf{P}\big{(}t_{k}^{+}\big{)}\big{]}\mathrm{q}[\mathbf{R} _{k}]\mathrm{q}[\xi(t_{k})]\mathrm{q}[\tau(t_{k})] \tag{18}\]  
The optimal solutions satisfied the Equation (19). \[\log\ \mathrm{q}(\kappa)=\mathrm{E}_{\Theta^{-\kappa}}\{\log[\mathrm{Pro}( \Theta,\mathbf{Z}_{1:k})]\}+\mathrm{C}_{\kappa} \tag{19}\]  
where \(\mathrm{E}[\bullet]\) is the expectation operation; \(\Theta^{-\kappa}\) refers to all the elements in \(\Theta\) except for \(\kappa\); \(\mathrm{C}_{\kappa}\) is a constant related to \(\kappa\). The logarithm of \(\mathrm{Pro}[\Theta,\mathbf{Z}_{1:k}]\) is calculated as below:  
\[ \begin{array}{ll}\log[\mathrm{Pro}(\Theta,\mathbf{Z}_{1:k})]=&-\frac{1}{2} \xi(t_{k})\{\mathbf{Z}(t_{k})-h[\mathbf{X}(t_{k})]\}-\frac{1}{2}[1-\xi(t_{k})] \mathbf{Z}^{T}(t_{k})\mathbf{R}_{k}^{-1}\mathbf{Z}(t_{k})\\ &-\frac{1}{2}\big{[}n_{Z}+u\big{(}t_{k}^{+}\big{)}+2\big{]}\log[\mathbf{R}_{k} ]-\frac{1}{2}\big{[}\mathbf{X}(t_{k})-\mathbf{X}(t_{k}^{+})\big{]}^{T} \mathbf{P}^{-1}\big{(}t_{k}^{+}\big{)}\big{[}\mathbf{X}(t_{k})-\mathbf{X} \big{(}t_{k}^{+}\big{)}\big{]}\\ &-\frac{1}{2}tr\big{[}\mathbf{U}\big{(}t_{k}^{+}\big{)}\mathbf{R}_{k}^{-1} \big{]}-\frac{1}{2}\big{[}n_{x}+t\big{(}t_{k}^{+}\big{)}+2\big{]}\log\big{|} \mathbf{P}\big{(}t_{k}^{+}\big{)}\big{|}\\ &-\frac{1}{2}tr\big{[}\mathbf{T}\big{(}t_{k}^{+}\big{)}\mathbf{P}^{-1}\big{(} t_{k}^{+}\big{)}\big{]}+\xi(t_{k})\log[\tau(t_{k})]\\ &+[1-\xi(t_{k})]\log[1-\tau(t_{k})]+\big{[}\mathfrak{a}\big{(}t_{k}^{+}\big{)} -1\big{]}\log[\tau(t_{k})]\\ &+[\beta(t_{k}^{+})-1]\log[1-\tau(t_{k})]+C_{\varepsilon}\end{array}  \tag{20}\]  
where \(n_{x}\) indicates the dimension of \(\mathbf{X}(t_{k})\); \(n_{Z}\) indicates the dimension of \(\mathbf{Z}(t_{k})\). (1) The optimal solutions of predicted error covariance and observation noise matrices. Suppose \(\kappa=\mathbf{P}\big{(}t_{k}^{+}\big{)}\); then, the logarithm of \(\mathrm{q}(\kappa)\) in iteration \(l+1\) is written as:  
\[\log q^{(l+1)}(\kappa)=-\frac{1}{2}\Big{[}n_{x}+t^{(l)}\big{(}t_{k}^{+}\big{)} +2\Big{]}\log\Big{|}\mathbf{P}^{(l)}\big{(}t_{k}^{+}\big{)}\Big{|}\ -\frac{1}{2}tr\Big{\{}\Big{[}\mathbf{A} _{k}^{(l)}+\mathbf{T}^{(l)}\big{(}t_{k}^{+}\big{)}\Big{]}\mathbf{P}^{-(l)} \big{(}t_{k}^{+}\big{)}\Big{\}}+\mathrm{C}_{\mathbf{P}} \tag{21}\]  
where \(\mathbf{A}_{k}^{(l)}=\mathbf{P}^{(l)}(t_{k})+\Big{[}\mathbf{X}^{(l)}(t_{k})- \mathbf{X}\big{(}t_{k}^{+}\big{)}\Big{]}\Big{[}\mathbf{X}^{(l)}(t_{k})- \mathbf{X}\big{(}t_{k}^{+}\big{)}\Big{]}^{T}\). The DOF and inverse scale matrix of \(\mathbf{P}\big{(}t_{k}^{+}\big{)}\) in iteration \(l+1\) are updated as:  
\[\left\{ \begin{array}{l}t^{(l+1)}(t_{k})=t^{(l)}\big{(}t_{k}^{+}\big{)}+1\\ \mathbf{T}^{(l+1)}(t_{k})=\mathbf{A}_{k}^{(l)}+\mathbf{T}^{(l)}\big{(}t_{k}^{+ }\big{)}\end{array} \right. \tag{22}\]  
Set \(\kappa=\mathbf{R}_{k}\); then, the logarithm of \(\mathrm{q}(\kappa)\) is calculated in iteration \(l+1\) as:  
\[ \begin{array}{ll}\log\mathrm{q}^{(l+1)}(\kappa)=&-\frac{1}{2}\Big{[}n_{Z}+u ^{(l)}\big{(}t_{k}^{+}\big{)}+2\Big{]}\log\Big{|}\mathbf{R}_{k}^{(l)}\Big{|} \\ &-\frac{1}{2}tr\Big{\{}\Big{[}\mathbf{B}_{k}^{(l)}+\mathbf{U}_{k}^{(l)}\big{(} t_{k}^{+}\big{)}\Big{]}\mathbf{R}_{k}^{-(l)}\Big{\}}+\mathrm{C}_{\mathbf{R}} \end{array}  \tag{23}\]  
where  
\[ \begin{array}{ll}\mathbf{B}_{k}^{(l)}=&\mathrm{E}^{(l)}[\xi(t_{k})]\mathrm{E} ^{(l)}\Big{\{}\{\mathbf{Z}(t_{k})-h[\mathbf{X}(t_{k})]\}\{\mathbf{Z}(t_{k})-h[ \mathbf{X}(t_{k})]\}^{T}\Big{\}}\\ &\qquad\qquad+\Big{\{}1-\mathrm{E}^{(l)}[\xi(t_{k})]\Big{\}}\mathrm{E}^{(l)} \big{[}\mathbf{Z}(t_{k})\mathbf{Z}^{T}(t_{k})\big{]}\end{array}