Skip to main content
. 2019 Dec 21;20(1):78. doi: 10.3390/s20010078
Algorithm 1 Distributed Relative-Range Measurement Update
Input: Range measurement and collaborating node state parametrized with the mean and the covariance: {zkR,x^kc, cov(xkc,xkc)} 
Output: Local state posterior, p(xk+1|xk,zkR) , and cross-covariance, cov(xk+1,xk+1c) 
1 for each particle i 
2   Evaluate the conditional distribution, p(xkc|xki) , Equations (13) and (14)
3   Evaluate the measurement likelihood, p(zkR|xki,xkc) , Equation (7)
4   Evaluate the marginal likelihood: p(zkR|xki)=p(zkR|xki,xkc)p(xkc|x1:ki,z1:k1R)dxkc 
5   Evaluate p(xkc,i|x1:ki,z1:kR) , using RBPF formulation [49,50]
6   Evaluate particle weight: w˜kiwk1i×p(zkR|xki) 
7   Time update step: p(xk+1i|xki,zkR) and p(xk+1c,i|x1:ki,z1:kR) 
8 end for
9 Normalize the particle weights: wki=w˜ki/iw˜ki 
10  Evaluate the mean and variance terms, x^k+1 and cov(xk+1,xk+1) , Equations (10) and (11).
11  Update state cross-covariance term: cov(xk+1,xk+1c) , Equation (12)
12  return p(xk+1|xk,zkR) and cov(xk+1,xk+1c)