In this section, we modify the dKF under Gauss–Markov CMN. First, we consider the state-space model of the navigation problem. Then, the dKF is designed under CMN assuming switch CFM. Finally, the main filter fuses the results of the sub-filters.
Sub-filters for CMN
The state equation representing the 2D human dynamics and related to the jth sub-filter 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 sub-filter, \({\varvec{w}}_n^{(j)} \sim {\mathcal {N}} ({\varvec{0}}, {\varvec{Q}}^{(j)})\) is noise in the jth sub-filter.
The observation equation corresponding to the data obtained by the jth sub-filter 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}}_{n-1}^{\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}}_{n-1}^{\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 time-correlated with system noise \({\varvec{w}}_n^{(j)}\) and the KF cannot be applied straightforwardly. To de-correlate 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 de-correlated 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 de-correlation, the algorithm of the sub-KF 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 sub-KFs 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 sub-KF 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 sub-filters 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.