## 3 Switched Variational Estimation Filtering in Underwater Navigation
### Proposed Optimal Variational Estimation in Navigation Information Acquisition
\begin{array}{ll}\log q^{(l+1)}(\kappa)=&-\frac{1}{2}\Big{\{}1-\mathrm{E}^{(l)}[ \xi(t_{k})]\Big{\}}\mathbf{Z}^{T}(t_{k})\mathrm{E}^{(l+1)}\Big{(}\mathbf{R}_{k }^{-1}\Big{)}\mathbf{Z}(t_{k})\\ &-\frac{1}{2}\mathrm{E}^{(l)}[\xi(t_{k})]\left\{\mathbf{Z}(t_{k})-h[\mathbf{X }(t_{k})]\right\}^{T}\times\mathrm{E}^{(l+1)}\Big{(}\mathbf{R}_{k}^{-1}\Big{)} \{\mathbf{Z}(t_{k})-h[\mathbf{X}(t_{k})]\}\\ &-\frac{1}{2}\big{[}\mathbf{X}(t_{k})-\mathbf{X}\big{(}t_{k}^{+}\big{)}\big{]} ^{T}\mathrm{E}^{(l+1)}\big{[}\mathbf{P}^{-1}\big{(}t_{k}^{+}\big{)}\big{]} \big{[}\mathbf{X}(t_{k})-\mathbf{X}\big{(}t_{k}^{+}\big{)}\big{]}+\mathrm{C}_{ x}\end{array}  \tag{33}\]  
In iteration \(l\) +1,the update of \(q[\mathbf{X}(t_{k})]\) is:  
\[q^{(l+1)}[\mathbf{X}(t_{k})]=N\big{(}\mathbf{X}(t_{k});\mathbf{X}\big{(}t_{k}^ {+}\big{)},\mathbf{P}\big{(}t_{k}^{+}\big{)}\big{)} \tag{34}\]  
**Case 1.2:** The measurement update is written based on the traditional Gaussian filter. \[\left\{ \begin{array}{ll}\mathbf{K}_{k}^{(l+1)}=\mathbf{P}^{(l+1)}\big{(}t_{ k}^{+}\big{)}\mathbf{H}_{k}^{T}\Big{[}\mathbf{H}_{k}\mathbf{P}^{(l+1)}\big{(}t_{k}^ {+}\big{)}\mathbf{H}_{k}^{T}+\mathbf{R}_{k}^{(l+1)}\Big{]}^{-1}\\ \mathbf{X}^{(l+1)}(t_{k})=\mathbf{X}\big{(}t_{k}^{+}\big{)}+\mathbf{K}_{k}^{( l+1)}\mathrm{E}^{(l)}[\xi(t_{k})]\Big{[}\mathbf{Z}(t_{k})-\mathbf{H}_{k}\mathbf{X}^{(l )}(t_{k})\Big{]}\\ \mathbf{P}^{(l+1)}(t_{k})=\mathbf{P}^{(l+1)}\big{(}t_{k}^{+}\big{)}-\mathbf{K}_ {k}^{(l+1)}\mathbf{H}_{k}\mathbf{P}^{(l+1)}\big{(}t_{k}^{+}\big{)}\end{array} \right. \tag{35}\]  
**Case 2.2:** The measurement update is rewritten below. The cubature points are selected by using Equation (36). \[\chi_{i,k}^{**(l)}=\mathbf{X}^{(l)}(t_{k})+\mathbf{C}_{i}^{**},\quad i=1,2, \ldots,2n_{x} \tag{36}\]  
where \(\mathbf{C}_{i}^{**}=\sqrt{n_{x}\mathbf{P}^{(l+1)}\big{(}t_{k}^{+}\big{)}} \big{[}\mathbf{I}_{x}\quad-\mathbf{I}_{x}\big{]}_{i}\). The predicted measurement vector is calculated below:  
\[\mathbf{Z}_{k}^{**(l)}=\frac{1}{2n_{x}}\sum_{i=1}^{2n_{x}}h\Big{(}\chi_{i,k}^{** (l)}\Big{)} \tag{37}\]  
The cross covariance matrix \(\mathbf{P}_{xz}^{(l+1)}\) and measurement error covariance matrix \(\mathbf{P}_{zz}^{(l+1)}\) are calculated by Equations (38) and (39). \[\mathbf{P}_{xz}^{(l+1)}=\frac{\mathrm{E}^{(l)}[\xi(t_{k})]}{2n_{x}}\sum_{i=1}^ {2n_{x}}\Big{[}\mathbf{X}^{(l)}(t_{k})-\chi_{i,k}^{**(l)}\Big{]}\Big{[}\mathbf{ Z}_{k}^{**(l)}-h\Big{(}\chi_{i,k}^{**(l)}\Big{)}\Big{]}+\frac{\left(1-\mathrm{E}^{(l)} \right)[\xi(t_{k})]}{2n_{x}}\sum_{i=1}^{2n_{x}}\Big{[}\mathbf{X}^{(l)}(t_{k})- \chi_{i,k}^{**(l)}\Big{]}\Big{(}\mathbf{Z}_{k}^{**(l)}\Big{)}^{T} \tag{38}\]  
\[\mathbf{P}_{zz}^{(l+1)}=\frac{\mathrm{E}^{(l)}[\xi[t_{k}]]}{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}\ +\Big{\{}1-\mathrm{E}^{(l)}[\xi(t_{k})] \Big{\}}\mathbf{Z}_{k}^{**(l)}\Big{(}\mathbf{Z}_{k}^{**(l)}\Big{)}^{T} \tag{39}\]  
**Remark 4. **_The derivation of Kalman gain \(\mathbf{K}_{k}^{(l+1)}\) can be expressed as:_  
\[\mathbf{K}_{k}^{(l+1)}=\mathbf{P}_{xz}^{(l+1)}\mathbf{P}_{zz}^{-(l+1)} \tag{40}\]  
_In iteration 1+1, \(\mathbf{X}(t_{k})\) and \(\mathbf{P}(t_{k})\) can be calculated as:_  
\[\mathbf{X}^{(l+1)}(t_{k})=\mathbf{X}\big{(}t_{k}^{+}\big{)}+\mathbf{K}_{k}^{(l+1 )}\Big{[}\mathbf{Z}(t_{k})-\mathbf{Z}_{k}^{**(l)}\Big{]} \tag{41}\]  
\[\mathbf{P}^{(l+1)}(t_{k})=\mathbf{P}^{(l+1)}\big{(}t_{k}^{+}\big{)}-\mathbf{K}_ {k}^{(l+1)}\mathbf{P}_{zz}^{(l+1)}\Big{(}\mathbf{K}_{k}^{(l+1)}\Big{)}^{T} \tag{42}\]  
(3)  
The optimal solutions of the random variable and acquisition probability.