Suppose a joint state representing a set of \(N_{n}\) nodes moving in a field
\[
\textbf{X}=
\begin{bmatrix}
\left(\textbf{x}^{1}\right)^{T} & \left(\textbf{x}^{2}\right)^{T} & \cdots & \left(\textbf{x}^{N_{n}}\right)^{T} \\
\end{bmatrix}
^{T}
%_{n\times 1}
\]
To track the joint state cooperatively, a filter propagates states from time \(k-1\) to \(k\) and updates estimates with all observations at time \(k\).
\begin{equation} \label{eq:cooperativeFormulation}
P\left(\textbf{X}_{k}|\textbf{Z}_{1:k}\right) \propto P\left(\textbf{Z}_{k}|\textbf{X}_{k}\right) \int P\left(\textbf{X}_{k}|\textbf{X}_{k-1}\right) P\left(\textbf{X}_{k-1}|\textbf{Z}_{1:k-1}\right)d\textbf{X}_{k-1}
\end{equation}
We make the following assumptions:
- every node moves independently in the field, from which we have: \(P\left(\textbf{X}_{k}|\textbf{X}_{k-1}\right)=\prod_{p=1}^{N_{n}}P\left(\textbf{x}_{k}^{p}|\textbf{x}_{k-1}^{p}\right)\).
- an egocentric position observation \(\textbf{z}_{k}^{p}\) regarding node \(p\) is only dependent on current state of the node \(\textbf{x}_{k}^{p}\).
- a relative range observation \(\textbf{z}_{k}^{p\rightarrow q}\ \left(p\neq q\right)\) is only conditional on the current state of two involved nodes, i.e. \(\textbf{x}_{k}^{p}\) and \(\textbf{x}_{k}^{q}\).
Therefore the observation component of Equation \eqref{eq:cooperativeFormulation} is able to be further factorised to absolute and relative observations.
\[
P\left(\textbf{Z}_{k}|\textbf{X}_{k}\right)=\left(\prod_{p=1}^{N_{n}}P\left(\textbf{z}_{k}^{p}|\textbf{x}_{k}^{p}\right)\right)\left(\prod_{p=1}^{N_{n}}\prod_{q=1}^{N_{n}}P\left(\textbf{z}_{k}^{p\rightarrow q}|\textbf{x}_{k}^{p},\textbf{x}_{k}^{q}\right)\right)
\]
where \(p\neq q\).
A marginal distribution \(P\left(\textbf{x}_{k}^{p}|\textbf{Z}_{1:k}\right)\) for node \(p\) at time \(k\) could be obtained by integrating with respect to the joint state of the rest nodes (denoted by \(\overline{\textbf{X}}_{k}\)) in the joint posterior in Equation \eqref{eq:cooperativeFormulation}. This is achieved by:
\begin{equation} \label{eq:marginalisation}
P\left(\textbf{x}_{k}^{p}|\textbf{Z}_{1:k}\right)=\int P\left(\textbf{X}_{k}|\textbf{Z}_{1:k}\right)d \overline{\textbf{X}}_{k}
\end{equation}
where
\(
\textbf{X}_{k}=
\begin{bmatrix}
\left(\textbf{x}_{k}^{p}\right)^{T} & \overline{\textbf{X}}^{T}_{k} \\
\end{bmatrix}
^{T}
\)
本站内容若无说明，则为原创
转载请注明，欢迎讨论和指正
bot.Seamus