In this section, we modify the dKF under Gauss–Markov CMN. First, we consider the statespace model of the navigation problem. Then, the dKF is designed under CMN assuming switch CFM. Finally, the main filter fuses the results of the subfilters.
Subfilters for CMN
The state equation representing the 2D human dynamics and related to the jth subfilter is described by:
$$\begin{aligned} {\varvec{x}}_n^{(j)} &= {{\varvec{F}}^{(j)}}{\varvec{x}}_{n  1}^{(j)} + {\varvec{w}}_n^{(j)}\\&= { \left[ {\begin{array}{*{20}{c}} 1&\quad {{T^{(j)}}}&\quad 0&\quad 0\\ 0&\quad 1&\quad 0&\quad 0\\ 0&\quad 0&\quad 1&\quad {{T^{(j)}}}\\ 0&\quad 0&\quad 0&\quad 1 \end{array}} \right] {\varvec{x}}_{n  1}^{(j)} + {\varvec{w}}_n^{(j)}} \end{aligned}$$
(1)
where the state vector is defined as
$$\begin{aligned} {\varvec{x}}_n^{(j)} = \left[ {\begin{array}{*{20}c} {\delta {\mathrm{Pos}}_n^{E{(j)}} } &\quad {\delta {\mathrm{Vel}}_n^{E{(j)}} } & \quad {\delta {\mathrm{Pos}}_n^{N{(j)}} } &\quad {\delta {\mathrm{Vel}}_n^{N{(j)}} } \\ \end{array}} \right] ^{\mathrm{T}}, \end{aligned}$$
in which \(( {\delta {\mathrm{Pos}}_n^{E{(j)}} ,\delta {\mathrm{Pos}}_n^{N{(j)}} })\) and \(( {\delta {\mathrm{Vel}}_n^{E{(j)}} ,\delta {\mathrm{Vel}}_n^{N{(j)}} } )\) are the position and velocity errors in east and north directions, \(T^{(j)}\) is the sample time for the jth subfilter, \({\varvec{w}}_n^{(j)} \sim {\mathcal {N}} ({\varvec{0}}, {\varvec{Q}}^{(j)})\) is noise in the jth subfilter.
The observation equation corresponding to the data obtained by the jth subfilter is written as
$$\begin{aligned} y_n^{(j)} &= \delta {\mathrm{r}}_n^{(j)} = {\mathrm{r}}_n^{I(j)} {\mathrm{r}}_n^{U(j)}\\ & = \frac{{\delta _{{\mathrm{x}}n}^{(j)}}}{{\sqrt{\delta _{{\mathrm{x}}n}^{{{(j)}^2}} + \delta _{{\mathrm{y}}n}^{{{(j)}^2}}} }}\delta {\mathrm{Pos}}_n^{E(j)} + \frac{{\delta _{{\mathrm{y}}n}^{(j)}}}{{\sqrt{\delta _{{\mathrm{x}}n}^{{{(j)}^2}} + \delta _{{\mathrm{y}}n}^{{{(j)}^2}}} }}\delta {\mathrm{Pos}}_n^{N(j)} + {\varvec{v}}_n^{(j)}\\ & = {\left[ {\begin{array}{*{20}{c}} {\frac{{\delta _{{\mathrm{x}}n}^{(j)}}}{{\sqrt{\delta _{{\mathrm{x}}n}^{{{(j)}^2}} + \delta _{{\mathrm{y}}n}^{{{(j)}^2}}} }}}\\ 0\\ {\frac{{\delta _{{\mathrm{y}}n}^{(j)}}}{{\sqrt{\delta _{{\mathrm{x}}n}^{{{(j)}^2}} + \delta _{{\mathrm{y}}n}^{{{(j)}^2}}} }}}\\ 0 \end{array}} \right] ^{\mathrm{T}}}{\varvec{x}}_n^{(j)} + {\varvec{v}}_n^{(j)}\\ & = {\varvec{H}}_n^{(j)}{\varvec{x}}_n^{(j)} + {\varvec{v}}_n^{(j)} \end{aligned}$$
(2)
where \(j \in [1,M]\), \(\delta _{{\mathrm {x}}n}^{(j)} = {\mathrm{Pos}}_n^{E,I}  x^{(j)}\), \(\delta _{{\mathrm {y}}n}^{(j)} = {\mathrm{Pos}}_n^{N,I}  y^{(j)}\), \(\left( {{\mathrm{Pos}}_n^{E,I} ,{\mathrm{Pos}}_n^{N,I}} \right)\) denotes INS positions in east and north directions, and \({\varvec{v}}_n\) is Gauss–Markov CMN represented with
$$\begin{aligned} {\varvec{v}}_n^{\left( j \right) } = \alpha _n^{\left( j \right) } {\varvec{v}}_{n1}^{\left( j \right) } + \gamma _n^{\left( j \right) } \end{aligned}$$
(3)
where \(\gamma _n^{\left( j \right) } \sim {\mathcal {N}} ({\varvec{0}},{\varvec{R}})\) is white Gaussian driving noise and \(\alpha _n^{\left( j \right) }\) is the CFM.
To address the effects of CMN and apply filtering algorithms, we use measurement differencing and write the new observation equation as
$$\begin{aligned} {\varvec{z}}_n^{\left( j \right) }&= {\varvec{y}}_n^{\left( j \right) }  \alpha _n^{\left( j \right) } {\varvec{y}}_{n1}^{\left( j \right) } \nonumber \\&= {\varvec{H}}_n^{\left( j \right) } {\varvec{x}}_n^{\left( j \right) } + {\varvec{v}}_n^{\left( j \right) }  \alpha _n^{\left( j \right) } {\varvec{H}}_{n  1}^{\left( j \right) } {\varvec{x}}_{n  1}^{\left( j \right) } \nonumber \\&\quad  \alpha _n^{\left( j \right) }{\varvec{v}}_{n  1}^{\left( j \right) } \end{aligned}$$
(4)
From (1) and (3) we obtain
$$\begin{aligned} {\varvec{x}}_{n  1}^{(j)}&= {{\varvec{F}}^{(j)^{1}}} ( {{\varvec{x}}_n^{(j)}  {\varvec{w}}_n^{(j)}} ) \end{aligned}$$
(5)
$$\begin{aligned} {\varvec{v}} _{n  1}^{(j)}&= {\alpha _n^{(j)^{1}} } ( {{\varvec{v}} _n^{(j)}  \gamma _n^{(j)}} ) \end{aligned}$$
(6)
and substitute them into (4), giving
$$\begin{aligned} {\varvec{z}}_n^{(j)}&= ( {\varvec{H}}_n^{(j)}  {\varvec{T}}_n^{(j)} ) {\varvec{x}}_n^{(j)} + {\varvec{T}}_n^{(j)} {\varvec{w}}_n^{(j)} + \gamma _n^{(j)} \nonumber \\&= {\varvec{D}}_n^{(j)} {\varvec{x}}_n^{(j)} + {\bar{\gamma }}_n^{(j)} \end{aligned}$$
(7)
where \({\varvec{T}}_n^{(j)} = \alpha _n^{(j)} {\varvec{H}}_n^{(j)} {\varvec{F}}^{(j)^{1}}\), \({\varvec{D}}_n^{(j)} = {\varvec{H}}_n^{(j)}  {\varvec{T}}_n^{(j)}\), \({{\bar{\gamma }}} _n^{(j)} ={\varvec{T}}_n^{(j)} {\varvec{w}}_n^{(j)} + \gamma _n^{(j)}\) and \({\bar{\gamma }}_n^{(j)} \sim {\mathcal {N}} ( {{\varvec{0}},{\bar{{\varvec{R}}}} } )\) is white Gaussian noise
$$\begin{aligned} {\bar{\gamma }}_n^{(j)} = {\varvec{T}}_n^{(j)} {\varvec{w}}_n^{(j)} + \gamma _n^{(j)} \end{aligned}$$
(8)
with the covariance
$$\begin{aligned} \overline{\varvec{R}} &= {\mathrm{E}}\{ {{\bar{\gamma }}} _{\mathrm{n}}^{({\mathrm{j}})}{{\bar{\gamma }}} _{\mathrm{n}}^{{{({\mathrm{j}})}^{\mathrm{T}}}}\} = {\varvec{T}}_{\mathrm{n}}^{({\mathrm{j}})}{{\varvec{Q}}^{({\mathrm{j}})}}{\varvec{T}}_{\mathrm{n}}^{{{({\mathrm{j}})}^{\mathrm{T}}}}+ {\varvec{R}}\\ & = {\varvec{T}}_n^{(j)}{\varvec{\Phi }}_n^{(j)} + {\varvec{R}} \end{aligned}$$
(9)
where \({\varvec{\Phi }}_n^{(j)} = {\varvec{Q}}^{(j)} {\varvec{T}}_n^{(j)^T}\). It follows from (8) that the observation noise \({\bar{\gamma }}_n^{(j)}\) is timecorrelated with system noise \({\varvec{w}}_n^{(j)}\) and the KF cannot be applied straightforwardly. To decorrelate noise, we follow Shmaliy et al. (2020) and modify the state equation (1) as
$$\begin{aligned} {\varvec{x}}_n^{(j)} &= {{\varvec{F}}^{(j)}}{\varvec{x}}_{n  1}^{(j)} + {\varvec{w}}_n^{(j)} + {\varvec{\beta }}_n^{(j)}[{\varvec{z}}_n^{(j)}  ({\varvec{D}}_n^{(j)}{\varvec{x}}_n^{(j)} + {\bar{{{\varvec{\gamma }}} }}_n^{(j)})]\\ & = ({\varvec{I}}  {\varvec{\beta }}_n^{(j)}{\varvec{D}}_n^{(j)}){{\varvec{F}}^{(j)}}{\varvec{x}}_{n  1}^{(j)} + {\varvec{\beta }}_n^{(j)}{\varvec{z}}_n^{(j)}\\ &\quad + ({\varvec{I}}  {\varvec{\beta }}_n^{(j)}{\varvec{D}}_n^{(j)}){\varvec{w}}_n^{(j)}  {\varvec{\beta }}_n^{(j)}{\bar{{{\varvec{\gamma }}} }}_n^{(j)}\\ & = {\varvec{A}}_n^{(j)}{\varvec{x}}_{n  1}^{(j)} + {\varvec{u}}_n^{(j)} + {\varvec{\eta }}_n^{(j)} \end{aligned}$$
(10)
where \({{\varvec{\eta }}_n^{(j)} } \sim {\mathcal {N}} ({\varvec{0}},{\varvec{\Theta }}_n^{(j)})\) has the covariance
$$\begin{aligned} {\varvec{\Theta }}_{\mathbf {n}}^{({\mathbf {j}})} &= {\mathbf {E}}\left\{ {[({\mathbf {I}}  {\varvec{\beta }}_{\mathbf {n}}^{({{j}})}{\mathbf {D}}_{\mathbf {n}}^{({{j}})}){\mathbf {w}}_{\mathbf {n}}^{({{j}})}  {\varvec{\beta }}_{\mathbf {n}}^{({{j}})}{ \bar{{{\boldsymbol{\gamma }}} }}_{\mathbf {n}}^{({{j}})}]} \right. \\ &\quad \left. {{{[({\mathbf {I}}  {\varvec{\beta }}_{\mathbf {n}}^{({{j}})}{\mathbf {D}}_{\mathbf {n}}^{({{j}})}){\mathbf {w}}_{\mathbf {n}}^{({{j}})}  {\varvec{\beta }}_{\mathbf {n}}^{({{j}})}{\bar {{{\boldsymbol{\gamma }}} }}_{\mathbf {n}}^{({{j}})}]}^{\mathrm{T}}}} \right\} \\ & = ({\mathbf {I}}  {\varvec{\beta }}_{\mathbf {n}}^{({{j}})}{\mathbf {H}}_{\mathbf {n}}^{({{j}})}){\mathbf {Q}}_{\mathbf {n}}^{({{j}})}{({\mathbf {I}}  {\varvec{\beta }}_{\mathbf {n}}^{({{j}})}{\mathbf {H}}_{\mathbf {n}}^{({{j}})})^{\mathrm{T}}}\\ &\quad + {\varvec{\beta }}_{\mathbf {n}}^{({{j}})}{\mathbf {R}}{({\varvec{\beta }}_{\mathbf {n}}^{({{j}})})^{\mathrm{T}}} \end{aligned}$$
(11)
The noise vectors \({{\varvec{\eta }}_n^{(j)} }\) and \({{{\bar{\gamma }}} _n^{(j)} }\) will be decorrelated if the conditions \({\mathrm {E}} \{ {{\varvec{\eta }}_n^{(j)} ( {{{\bar{\gamma }}} _n^{(j)} } )^{\mathrm{T}} } \} = 0\) is satisfied that can be achieved with
$$\begin{aligned} {\varvec{\beta }}_n^{(j)}&= \Phi _n^{(j)} ( {{\varvec{H}}_n^{(j)} \Phi _n^{(j)} + {\varvec{R}}} )^{  1}\,, \end{aligned}$$
(12)
$$\begin{aligned} {\varvec{\Theta} }_n^{(j)}&= ( {{\varvec{I}}  {\varvec{\beta }}_n^{(j)} {\varvec{H}}_n^{(j)} } ){\varvec{Q}}^{( j )} ( {{\varvec{I}}  {\varvec{\beta }}_n^{(j)} {\varvec{D}}_n^{(j)} } )^{\mathrm{T}} \end{aligned}$$
(13)
Provided the decorrelation, the algorithm of the subKF method operating under CMN is described in Algorithm 1. Unlike the standard KF, Algorithm 1 requires the CFM \(\alpha _n^{(j)}\) at each n. To set \(\alpha _n^{(j)}\) properly, we take notice of possible time variations in \(\alpha _n^{(j)}\), and make CMF switching by the following steps:

Set several possible values of \(\alpha _n^{(j),i}, i\in [1,q]\).

Run q subKFs with \(\alpha _n^{(j),i}, i\in [1,q]\) in parallel.

Compute the Mahalanobis distance (Mahalanobis, 1936)
$$\begin{aligned} L_n^{\alpha _n^{(j),i} } = ( {{\varvec{z}}_n^{(j)}  {\varvec{D}}_n^{(j)} {{\hat{\varvec{x}}}}_n^{(j)} } )^T {{\varvec{R}}^{(j)^{1}} } ( {{\varvec{z}}_n^{(j)}  {\varvec{D}}_n^{(j)} {{\hat{\varvec{x}}}}_n^{(j)} } ) \end{aligned}$$
(14)

Find \(\alpha _{n,{\mathrm {opt}}}^{(j)}\) by solving the minimization problem
$$\begin{aligned} \alpha _{n,{\mathrm {opt}}}^{(j)} = \mathop {\arg \min }\limits _{\alpha _n^{(j),i} } {L_n^{\alpha ^{(j),i} } } \end{aligned}$$
(15)
A pseudo code of the subKF for CMN with switch CFM is listed as Algorithm 2 and the structure of this filter is shown in Fig. 3. Having selected a proper range for CFM, this algorithm determines \(\alpha _{n,{\mathrm {opt}}}^{(j)}\), which is further used in the main filter.
Distributed KF for CMN with switch CMN
The dKF algorithm used in the proposed navigation system is responsible for fusing data collected from local subfilters and estimating the object position \(\hat{\varvec{x}}_n\) and localization error covariance \({\varvec{P}}_n\) as
$$\begin{aligned} \hat{\varvec{x}}_n&= {\varvec{P}}_n ( {\varvec{P}}_n^{(1)^{1}} \hat{\varvec{x}}_n^{(1)} + {\varvec{P}}_n^{(2)^{1}} \hat{\varvec{x}}_n^{(2)} \nonumber \\&\quad + \dots + {\varvec{P}}_n^{(M)^{1}} \hat{\varvec{x}}_n^{(M)}) \,, \end{aligned}$$
(16)
$$\begin{aligned} {\varvec{P}}_n^{1}&= {\varvec{P}}_n^{(1)^{1}} + {\varvec{P}}_n^{(2)^{1}} + \cdots + {\varvec{P}}_n^{(M)^{1}} \end{aligned}$$
(17)
A pseudo code of the main dKF for CMN with switch CFM is listed as Algorithm 3.