## 3 Distributed Cubature Kalman Filter
### Distributed Cubature Kalman Filter  
Suppose that the state \(x_{k-1}\) is approximated by sensor \(i\) at time \(k-1\) as follows,  
\[p(x_{k-1}|z_{i,k-1})=\mathbb{N}(\hat{x}_{i,k-1},\hat{P}_{i,k-1}), \tag{32}\]  
where \(\mathbb{N}(x,P)\) denotes the Gaussian distribution with mean \(x\) and covariance \(P\). The predictive distribution \(p(x_{k}|z_{i,k-1})=\mathbb{N}(\hat{x}_{i,k},\hat{P}_{i,k})\) can be obtained by the prediction step of CKE. To be specific, a set of cubature points [13] can be provided by:  
\[X_{i,t,k-1}= S_{i,k-1}\xi_{t}+\hat{x}_{i,k-1}, \tag{33}\] \[X_{i,t,k}^{*}= f(X_{i,t,k-1}), \tag{34}\]  
where the basic cubature point is given by \(\xi_{t}=\sqrt{\frac{m}{2}}\times[1]_{t}\), \(t=1,\ldots,m\), \(m=2n_{x}\) and \([1]_{t}\) denotes the \(t\)-th element of set \([1]\). For example, let \([1]\in\mathbb{R}^{2}\), then it represents the set \(\left\{ \begin{bmatrix}1\\ 0\end{bmatrix} , \begin{bmatrix}0\\ 1\end{bmatrix} , \begin{bmatrix}-1\\ 0\end{bmatrix} , \begin{bmatrix}0\\ -1\end{bmatrix} \right\}\). Then, the predicted state and covariance are given by:  
\[\tilde{x}_{i,k}= \frac{1}{m}\sum_{t=1}^{m}X_{i,t,k}^{*}, \tag{35}\] \[\tilde{P}_{i,k}= \frac{1}{m}\sum_{t=1}^{m}X_{i,t,k}^{*}X_{i,t,k}^{*\top}-\tilde{x }_{i,k}\tilde{x}_{i,k}^{\top}+Q_{k-1}. \tag{36}\]  
Denote \(\tilde{P}_{i,k}=\tilde{S}_{i,k}\tilde{S}_{i,k}^{\top}\); under the assumption that these errors can be well approximated by the Gaussian, the prediction measurement can be obtained as follows,  
\[\tilde{z}_{i,k}=\frac{1}{m}\sum_{t=1}^{m}\tilde{Z}_{i,t,k}. \tag{37}\]  
where the set of cubature points \(\tilde{Z}_{i,t,k},t=1,\ldots,m\) is given by:  
\[\tilde{X}_{i,t,k}= \tilde{S}_{i,k}\xi_{t}+\tilde{x}_{i,k}, \tag{38}\] \[\tilde{Z}_{i,t,k}= h_{i}(\tilde{X}_{i,t,k}). \tag{39}\]  
In the Bayesian framework, these prediction means and covariances will be incorporated in the procedure as prior information of the state to propel the measurement update. Based on Equations (26) and (27), the local posterior can be given as follows,  
\[\tilde{x}_{i,k}= \tilde{x}_{i,k}+P_{i,k,xz}P_{i,k,xz}^{-1}(z_{i,k}-\tilde{z}_{i,k}) \tag{40}\]\[\tilde{P}_{i,k}= \tilde{P}_{k}+P_{i,k,xz}P_{i,k,zz}^{-1}P_{i,k,xz}^{\top}. \tag{41}\]  
Different from the standard cubature Kalman filter, the innovation covariance matrix \(P_{i,zz,k}\) and cross-covariance matrix \(P_{i,xz,k}\) of node \(i\) can be given according to (28) and (29) as follows,  
\[P_{i,zz,k}= N\left[\frac{1}{m}\sum_{t=1}^{m}\tilde{Z}_{i,t,k}\tilde{Z}_{i,t,k}^{ \top}-\tilde{z}_{i,k}\tilde{z}_{i,k}^{\top}\right]+R_{i,k}, \tag{42}\] \[P_{i,xz,k}= N\left[\frac{1}{m}\sum_{t=1}^{m}\tilde{X}_{i,t,k}\tilde{Z}_{i,t,k}^{ \top}-\tilde{x}_{i,k}\tilde{z}_{i,k}^{\top}\right]. \tag{43}\]  
By the consensus of Gaussian distributions (30), the global estimate can be approximated as:  
\[(P_{i}^{s+1})^{-1}\delta_{i}^{s+1}=\sum_{j\in\mathcal{N}_{i}}a_{ ij,k}(P_{j}^{s})^{-1}\delta_{j}^{s},\ \ s=1,2,\ldots \tag{44}\] \[\tilde{P}_{i,k}= \frac{1}{m}\sum_{t=1}^{m}X_{i,t,k}^{*}X_{i,t,k}^{*\top}-\tilde{x }_{i,k}\tilde{x}_{i,k}^{\top}+Q_{k-1}. \tag{36}\]0  
With the iterations (44) and (45), we can get the final estimation of state in each sensor. Meanwhile, the iterative estimation will approximate to the global solution of Problem (20) because of the convergence of the average consensus of PDFs as \(S\rightarrow\infty\). In practice, the convergence will not be achieved fully, because the total number of iterations \(S\) is finite. Therefore, the distributed implementation will not perform as well as the centralized one. We summarize the distributed cubature Kalman filter in Algorithm 1. In (36), \(Q_{k}\) can be chosen as a sufficiently small constant matrix. Notice that the initialization of all the local estimates is exactly the same mean of the initial state. However, in practice, it is not easy to let every sensor know the prior knowledge. A more suitable setting is \(x_{i,0}=0,\forall i\in\mathcal{V}\) and \(P_{i,0}=0,\forall i\in\mathcal{V}\), which means that there does not exist prior knowledge.