Bearings-Only Tracking in Modified Polar Coordinate System : Initialization of the Particle Filter and Posterior Cramér-Rao Bound

Transcription

1 Bearings-Only Tracking in Modified Polar Coordinate System : Initialization of the Particle Filter and Posterior Cramér-Rao Bound Thomas Brehard (IRISA/CNRS), Jean-Pierre Le Cadre (IRISA/CNRS). Journée AS 67 Méthodes Particulaires Applications du Filtrage Particulaire ENST 3 December 2003

2 Summary 1. What is the Bearings-Only Tracking problem? 2. Solving using particle filtering algorithm From cartesian to modified polar coordinate system. 4. Initialization of the particle filtering algorithm. 5. Posterior Cramér-Rao bound. 6. Conclusion.

3 1. What is the Bearings-Only Tracking problem? (1/2) FIG. 1 Trajectories of the observer (pink) and the target (blue) and simulated bearing measurements. X(t) = X 1 (t) X 2 (t) X 3 (t) X 4 (t) = v x (t) v y (t) r x (t) r y (t) Z t tan ( ) ry (t) r x (t) X t is the target state at time t composed of relative velocity and position of the target in the x y plane Z t is the bearing measurement received at time t. Problem : Estimate X t using {Z 1,..., Z t }.

4 1. What is the bearings-only tracking problem?(2/2) The stochastic system : { Xt+1 = AX t + HU t + W t Z t = G(X t ) + V t where : X t is the target state at time t composed of relative velocity and position of the target in the x y plane, Z t the bearing measurement received at time t, U t is the known difference between observer velocity at time t + 1 and t. V t has a center normal distribution with variance σ v known. W t has a center normal distribution with covariance matrix Q known. This is a non-linear filtering problem which can be solved using particle filtering algorithm!

5 2. Solving using particle filtering algorithm... At each step of time : 1. Propagating the set of particles using the state equation. 2. Weighting each of the particles using the measurement equation. 3. Resampling step. Reference : Doucet et al. (2001) Problem Particles must be properly initialized!

6 3. From cartesian to modified polar coordinate system (1/3) There is an unobservability problem hidden in the cartesian formulation of the system : { Xt+1 = AX t + HU t + W t Problem Z t = G(X t ) + V t The range is unobservable until the observer has maneuvered. Solution Reference : Aidala and Hammel (1983) A coordinate system more suited to the problem : the modified polar coordinate system

7 3. From cartesian to modified polar coordinate system (2/3) The modified polar coordinates We can show that : Y (t) = Y 1 (t) Y 2 (t) Y 3 (t) Y 4 (t) = β(t) ṙ(t) r(t) β(t) 1 r(t) Y 4 (t) is unobservable until the observer has not maneuvered. Y r (t) = Y 1(t) β(t) Y 2 (t) = ṙ(t) r(t) is always observable. Y 3 (t) β(t)

8 3. From cartesian to modified polar coordinate system (3/3) Before the observer maneuvers, the stochastic system in modified polar coordinate system is where Y r t+1 = F (Y r t, W t ), Y 4 (t + 1) = H(Y t, W t ), Z t = Y 3 (t) + V t W t = Y 4 (t)w t and W t has a center normal distribution with covariance matrix Q known. An interesting model : This is a non linear filtering problem with unknown covariance state ( Y 4 (t) is unknown!).

9 4. Initialization of the particle filtering algorithm (1/7) 2 key ideas : We can proove that if the target has a deterministic trajectory then for all k ( ) Z(t 0 + k) = Y 3 (t 0 ) + tan 1 kδt Y 1 (t 0 ) + V k 1 + kδ t Y 2 (t 0 ) It is just an optimization problem. The observable components Yt r 0 can be estimated using the set of measurements {Z t0,..., Z t0 +K}. We only assume a prior information on the unobservable component Y 4 (t 0 ) : 1 R max Y 4 (t 0 ) 1 R min

10 4. Initialization of the particle filtering algorithm (2/7) Initialization of the particle filtering algorithm Wait until time t 0 + K, the particule i is initialized by where 1 (t 0 ) 2 (t 0 ) 3 (t 0 ) Y (i) Y (i) Y (i) CA(Ŷ r t 0 ), Ŷ t r 0 is computed using a Gauss-Newton optimization algorithm (initialized by linear regression). CA(Ŷ t r 0 ) is the confidence area of Ŷ t r 0 (approximated by an hyperellipsoid). Y (i) 4 (t 0 ) 1 U[R min, R max ].

11 4. Initialization of the particle filtering algorithm (3/7) Two important points Before the observer maneuvers, Y (i) 1 (t) the observable component of the particles Y (i) 2 (t) and the unobservable Y (i) 3 (t) component of the particles Y (i) 4 (t) must be resampled independently! How the initialization time K can be fixed? The particle filtering algorithm is initialized as soon as the volume of the confidence area for Ŷ r t 0 is sufficiently small to be filled by N particles.

12 4. Initialization of the particle filtering algorithm (4/7) X target 0 = Simulation scenario 10 ms 1 2 ms 1 0 m m, Xobs 0 = 6 ms 1 3 ms m 0 m The observer follows a leg-by-leg trajectory. His velocity vector is constant on each leg and modified at the two following instants : ( ) ( V obs x (2100) 10 ms 1 Vx obs = (2100) 2 ms 1 ) ( V obs, x (3900) Vx obs (3900). ) ( 10 ms 1 = 2 ms 1 ). t = 1800s t = 3600s t = 5400s Trajectories of the observer (pink) and the target (blue).

13 4. Initialization of the particle filtering algorithm (5/7) Simulation scenario Simulated bearing measurements. The measurement standard deviation is 0.05 rad (about 3 deg). Parameters for the particle filtering algorithm Number of particles : 10000, Sampling threshold : 0.5, The single assumption : R min = 1000 m and R max = m.

14 4. Initialization of the particle filtering algorithm (6/7) Simulation results in modified polar coordinates Y 1 (t) Y 2 (t) Y 3 (t) Y 4 (t) True value (blue), Estimates (red), Confidence (green)

15 4. Initialization of the particle filtering algorithm (7/7) Simulation results in cartesian system Conclusion t = 1800s t = 3600s t = 5400s Trajectory of the observer (pink), trajectory of the true target (blue), particles (red), confidence for the estimate (green). We have initialized the particle filtering algorithm using a weak prior on range. Performance analysis in polar modified coordinate system.

16 5. Posterior Cramér-Rao Bound (1/9) Next step : The performance analysis in modified polar coordinate system. Tool : The Posterior Cramér-Rao Bound (PCRB). The PCRB gives a lower bound for the Mean Square Error

17 5. Posterior Cramér-Rao Bound (2/9) Notations : Y 0:t = {Y 0,..., Y t } and Z 0:t = {Z 0,..., Z t }. The bias where g(z 0:t ) is the estimator of Y 0:t B(Y 0:t ) = E (g(z 0:t Y 0:t )) Y 0:t The asymptotic bias assumption lim Y i (j) Y + i B(Y 0:t )p(y 0:t ) = lim Y i (j) Y i B(Y 0:t )p(y 0:t ) i {1,..., n y } and j {0,..., t} where Y i is the state space of Y i (j) for all j in {0,..., t}, Y i and Y + i are the endpoints of Y i

18 5. Posterior Cramér-Rao Bound (3/9) Proposition 1 Under the asymptotic bias assumption, where ECM 0:t J 1 0:t ECM 0:t = E{(Y 0:t g(z 0:t ))(Y 0:t g(z 0:t )) t } J 0:t = E{ Y 0:t Y 0:t ln p(z 0:t, Y 0:t )} and g(z 0:t ) is the estimator of Y 0:t. In the filtering context, we only want to approximate the right lower block of J 0:t noted J t.

19 5. Posterior Cramér-Rao Bound (4/9) Using Tichavsky et al. results, we have where Conclusion : J t+1 = D 22 t D 21 t (J t + D 11 t ) 1 D 12 t D 11 t = E{ Yt ln p(y t+1 Y t ) t Y t ln p(y t+1 Y t )}}, D 21 t = E{ Yt+1 ln p(y t+1 Y t ) t Y t ln p(y t+1 Y t )}, D 12 t = E{ Yt+1 ln p(y t+1 Y t ) t Y t+1 ln p(y t+1 Y t )}, D 22 t = E{ Yt+1 ln p(y t+1 Y t ) t Y t+1 ln p(y t+1 Y t )} + E{ Yt+1 ln p(z t+1 Y t+1 ) t Y t+1 ln p(z t+1 Y t+1 )}. Rq : Dt 11, Dt 12, Dt 22, Dt 21 Cramér-Rao Bound. J t can be computed recursively. are approximated using Monte Carlo method and J(t 0 ) is obtained by a

20 5. Posterior Cramér-Rao Bound (5/9) Simulation results Y 1 (t) Y 2 (t) Y 3 (t) Y 4 (t) Problem : Hypothesis : (Mean square error in blue, PCRB in red) The PCRB seems over optimistic... Is the asymptotic bias assumption true? Is J 0:t ill-conditionned due to range unobservability?

21 5. Posterior Cramér-Rao Bound (6/9) Is the asymptotic bias assumption true? A more general bound : where ECM 0:t C 0:t J 1 0:t C t 0:t C 0:t = E{(g(Z 0:t ) Y 0:t ) t Y 0:t ln p(z 0:t, Y 0:t )}. Remarks : C 0:t can be approximated using Monte Carlo approximation. The recursive formulation of the PCRB is no longer valid.

22 5. Posterior Cramér-Rao Bound (7/9) Is the asymptotic bias assumption true? Simulation results Y 1 (t) Y 2 (t) Y 3 (t) Y 4 (t) Solution : (Mean square error in blue, classical PCRB in red, new PCRB in green) The asymptotic bias assumption is not true in the bearings-only tracking problem.

23 5. Posterior Cramér-Rao Bound (8/9) Problem : The posterior Cramér-Rao bound seems over optimistic... Hypothesis : Is the asymptotic bias assumption true? No Is J 0:t ill-conditionned due to range unobservability?

24 5. Posterior Cramér-Rao Bound (9/9) Is J 0:t ill-conditionned due to range unobservability? Idea : We only construct a bound for the observable components of the state Y r t. We can proove that : ECM r 0:t E{C 0:t (Y 4 (t 0 ))J 1 0:t (Y 4 (t 0 ))C t 0:t(Y 4 (t 0 ))}. where J 0:t (Y 4 (t 0 )) = E{ Y 0:t r Y ln p 0:t r Y 4 (t 0 )(Z 0:t, Y0:t) Y r 4 (t 0 )}, C 0:t (Y 4 (t 0 )) = E{(g r (Z 0:t ) Y0:t) r t Y ln p 0:t r Y 4 (t 0 )(Z 0:t, Y0:t) Y r 4 (t 0 )}. It is possible to approximate J 0:t (Y 4 (t 0 )) and C 0:t (Y 4 (t 0 )).

25 6. Conclusion We have proposed : An initialization method for particle filtering algorithm using modifed polar coordinates using a weak prior on range. A new interpretation of the Bearings-Only Tracking problem. A realistic posterior Cramér-Rao bound in modified polar coordinate system. Perspectives : 3D target tracking. Maneuvering target.