Multi-estimate extraction for SMC-PHD
Posted on
This post is based on Li, T., Corchado, J. M., Sun, S., & Fan, H. (2017). Multi-EAP: Extended EAP for multi-estimate extraction for SMC-PHD filter. Chinese Journal of Aeronautics, 30(1), 368–379.
A RFS (random finite sets) variable is a r.v. that takes values as unordered finite sets. The cardinality of a RFS variable $X$ is random and modelled by a discrete distribution $\rho(n)=\Pr(\vert X\vert=n)$. The RFS $X$ is specified entirely by its cardinality distribution $\rho(n)$ and a family of symmetric joint dist. $p_n(x_1,\ldots,x_n)$ that characterize the distribution of its elements over the state space, conditioned on the set cardinality $n$.
The PDF of an RFS variable $X$ is denoted $f(X)$ and defined as
\[f(\{x_1,\ldots,x_n\})=n!\cdot\rho(n)\cdot p_n(x_1,\ldots,x_n)\,.\]Code Reading
target state: $x_k=[p_{x,k},\dot p_{x,k},p_{y,k},\dot p_{y,k}, w_k]^T$
coordinated-turn state transition model:
\[x_k = F(\omega_{k-1}) + G\nu_{k-1}\]F = @(a)[1 sin(a(5,:)*T)/a(5,:) 0 -((1-cos(a(5,:)*T))/a(5,:)) 0
0 cos(a(5,:)*T) 0 -sin(a(5,:)*T) 0
0 (1-cos(a(5,:)*T))/a(5,:) 1 sin(a(5,:)*T)/a(5,:) 0
0 sin(a(5,:)*T) 0 cos(a(5,:)*T) 0
0 0 0 0 1]*a;
Noise.vel = [15; 15];
Noise.turn= pi/180;