Algorithm 1 Distributed Relative-Range Measurement Update |
Input: |
Range measurement and collaborating node state parametrized with the mean and the covariance:
|
Output: |
Local state posterior, , and cross-covariance,
|
1 for each particle
|
2 Evaluate the conditional distribution, , Equations (13) and (14) |
3 Evaluate the measurement likelihood, , Equation (7) |
4 Evaluate the marginal likelihood:
|
5 Evaluate , using RBPF formulation [49,50] |
6 Evaluate particle weight:
|
7 Time update step: and
|
8 end for
|
9 Normalize the particle weights:
|
10 Evaluate the mean and variance terms, and , Equations (10) and (11). |
11 Update state cross-covariance term: , Equation (12) |
12 return
and
|