|
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
|