Robotics 2 Target Tracking. Giorgio Grisetti, Cyrill Stachniss, Kai Arras, Wolfram Burgard

Transcription

1 Robotics 2 Target Tracking Giorgio Grisetti, Cyrill Stachniss, Kai Arras, Wolfram Burgard

2 Linear Dynamical System (LDS) Stochastic process governed by is the state vector is the input vector is the process noise is the system matrix is the input gain The system can be observed through is the measurement vector is the measurement noise is the measurement matrix

3 Discrete Time LDS Continuous model are difficult to realize Algorithms work at discrete time steps Measurements are acquired with certain rates In practice, discrete models are employed Discrete-time LDS are governed by is the state transition matrix is the discrete-time input gain Same observation function of continuous models

4 Discrete Time LDS Continuous model are difficult to realize Algorithms work at discrete time steps Measurements are acquired with certain rates In practice, discrete models are employed Discrete-time LDS are governed by In target tracking, the input is unknown! is the state transition matrix is the discrete-time input gain Same observation function of continuous models

5 LDS Example Throwing ball We want to throw a ball and compute its trajectory This can be easily done with an LDS The ball s state shall be represented as We ignore winds but consider the gravity force g No floor constraints We observe the ball with a noise-free position sensor

6 LDS Example Throwing ball Throwing a ball from s with initial velocity v Consider only the gravity force, g, of the ball y o s v x State vector Process matrices Initial state Input vector (scalar) Measurement vector Measurement matrix

7 LDS Example Throwing ball Initial State No noise

8 LDS Example Throwing ball System evolution Observations

9 LDS Example Throwing ball System evolution Observations

10 LDS Example Throwing ball System evolution Observations

11 LDS Example Throwing ball System evolution Observations

12 LDS Example Throwing ball System evolution Observations

13 LDS Example Throwing ball System evolution Observations

14 LDS Example Throwing ball System evolution Observations

15 LDS Example Throwing ball System evolution Observations

16 LDS Example Throwing ball System evolution Observations

17 LDS Example Throwing ball System evolution Observations

18 LDS Example Throwing ball System evolution Observations

19 LDS Example Throwing ball System evolution Observations

20 LDS Example Throwing ball Initial State It s windy and our sensor is imperfect: let s add Gaussian process and observation noise

21 LDS Example Throwing ball System evolution Observations

22 LDS Example Throwing ball System evolution Observations

23 LDS Example Throwing ball System evolution Observations

24 LDS Example Throwing ball System evolution Observations

25 LDS Example Throwing ball System evolution Observations

26 LDS Example Throwing ball System evolution Observations

27 LDS Example Throwing ball System evolution Observations

28 LDS Example Throwing ball System evolution Observations

29 LDS Example Throwing ball System evolution Observations

30 LDS Example Throwing ball System evolution Observations

31 LDS Example Throwing ball System evolution Observations

32 LDS Example Throwing ball System evolution Observations

33 Target Tracking Tracking is the estimation of the state of a moving object based on remote measurements. [Bar-Shalom] Detection is Knowing the presence of an object Tracking is Maintaining a state of an object over time Tracking maintains the object s state and identity despite detection errors (false negatives, false alarms), occlusions, and in the presence of other objects

34 Target Tracking Problem Statement Given (Linear/nonlinear) dynamical system model External measurements (from some sort of sensor) Wanted System state estimate (e.g. position, velocity, acceleration, ) Problems Track maintenance (i.e. creation, occlusion, deletion) Multiple targets Data association

35 Target Tracking Long History, Many Applications Fleet Management Surveillance Air Traffic Control People Tracking, HRI Motion Capture Military Applications

36 Tracking cycle

37 Tracking cycle Kalman filter motion model predicted state posterior state innovation from matched landmarks observation model predicted measurements in sensor coordinates targets raw sensory data

38 Kalman Filter (KF) Consider a discrete time LDS with dynamic equation where is a process noise The measurement equation is where is a measurement noise The initial state is generally unknown and modeled as a Gaussian random variable State estimate Covariance estimate

39 KF Cycle: State prediction State prediction In target tracking, no a priori knowledge of the dynamic equation is generally available Instead, different target motion models are used Brownian motion model Constant velocity model Constant acceleration model More advanced models (problem related)

40 Motion Models: Brownian No motion assumption Useful to describe stop-and-go motion behavior State representation Ball example Initial state Transition matrix

41 Motion Models: Brownian No motion assumption Useful to describe stop-and-go motion behavior State representation Ball example Initial state Transition matrix State prediction: Uncertainty grows

42 Motion Models: Brownian No motion assumption Useful to describe stop-and-go motion behavior State representation Ball example Initial state Transition matrix State prediction: Uncertainty grows

43 Motion Models: Brownian No motion assumption Useful to describe stop-and-go motion behavior State representation Ball example Initial state Transition matrix State prediction: Uncertainty grows

44 Motion Models: Brownian No motion assumption Useful to describe stop-and-go motion behavior State representation Ball example Initial state Transition matrix State prediction: Uncertainty grows

45 MMs: Constant Velocity Constant target velocity assumption Useful to model smooth target motion State representation Ball example Initial state Transition matrix

46 MMs: Constant Velocity Constant target velocity assumption Useful to model smooth target motion State representation Ball example Initial state Transition matrix State prediction: Linear target motion Uncertainty grows

47 MMs: Constant Velocity Constant target velocity assumption Useful to model smooth target motion State representation Ball example Initial state Transition matrix State prediction: Linear target motion Uncertainty grows

48 MMs: Constant Velocity Constant target velocity assumption Useful to model smooth target motion State representation Ball example Initial state Transition matrix State prediction: Linear target motion Uncertainty grows

49 MMs: Constant Velocity Constant target velocity assumption Useful to model smooth target motion State representation Ball example Initial state Transition matrix State prediction: Linear target motion Uncertainty grows

50 MMs: Constant Acceleration Constant target acceleration assumed Useful to model target motion that is smooth in position and velocity changes State representation Ball example Initial state Transition matrix

51 MMs: Constant Acceleration Constant target acceleration assumed Useful to model target motion that is smooth in position and velocity changes State representation Ball example Initial state Transition matrix State prediction: Non-linear motion Uncertainty grows

52 MMs: Constant Acceleration Constant target acceleration assumed Useful to model target motion that is smooth in position and velocity changes State representation Ball example Initial state Transition matrix State prediction: Non-linear motion Uncertainty grows

53 MMs: Constant Acceleration Constant target acceleration assumed Useful to model target motion that is smooth in position and velocity changes State representation Ball example Initial state Transition matrix State prediction: Non-linear motion Uncertainty grows

54 MMs: Constant Acceleration Constant target acceleration assumed Useful to model target motion that is smooth in position and velocity changes State representation Ball example Initial state Transition matrix State prediction: Non-linear motion Uncertainty grows

55 MMs: Constant Acceleration Constant target acceleration assumed Useful to model target motion that is smooth in position and velocity changes State representation Ball example Initial state Transition matrix

56 MMs: Constant Acceleration Constant target acceleration assumed Useful to model target motion that is smooth in position and velocity changes State representation Ball example Initial state Transition matrix State prediction: Non-linear motion Uncertainty grows

57 MMs: Constant Acceleration Constant target acceleration assumed Useful to model target motion that is smooth in position and velocity changes State representation Ball example Initial state Transition matrix State prediction: Non-linear motion Uncertainty grows

58 MMs: Constant Acceleration Constant target acceleration assumed Useful to model target motion that is smooth in position and velocity changes State representation Ball example Initial state Transition matrix State prediction: Non-linear motion Uncertainty grows

59 MMs: Constant Acceleration Constant target acceleration assumed Useful to model target motion that is smooth in position and velocity changes State representation Ball example Initial state Transition matrix State prediction: Non-linear motion Uncertainty grows

60 KF Cycle: Measurement Predict. Measurement prediction Observation Typically, only the target position is observed. The measurement matrix is then Note: One can also observe (not in this course) Velocity (Doppler radar) Acceleration (accelerometers)

61 KF Cycle: Data Association Once measurements are predicted and observed, we have to associate them with each other This is basically resolving the origin uncertainty of observations Data association is typically done in the sensor reference frame Data association can be a hard problem and many advanced techniques exist More on this later in this course

62 KF Cycle: Data Association Step 1: Compute the pairing difference and its associated uncertainty The difference between predicted measurement and observation is called innovation The associated covariance estimate is called the innovation covariance The prediction-observation pair is often called pairing

63 KF Cycle: Data Association Step 2: Check if the pairing is statistically compatible Compute the Mahalanobis distance Compare it against the proper threshold from an cumulative ( chi square ) distribution Significance level Degrees of freedom Compatibility on level α is finally given if this is true

64 KF Cycle: Data Association Constant velocity model Process noise Measurement noise No false alarm No problem

65 KF Cycle: Data Association Constant velocity model Process noise Measurement noise Uniform false alarm False alarm rate = 3 Ambiguity: several observations in the validation gate

66 KF Cycle: Data Association Constant velocity model Process noise Measurement noise Uniform false alarm False alarm rate = 3 Wrong association as closest observation is false alarm

67 KF Cycle: Update Computation of the Kalman gain State and state covariance update

68 Track management: Naïve Creation When to create a new track? What is the initial state? Occlusion/deletion When to delete a track? Is it just occluded? Two heuristics Greedy initialization Every observation not associated is a new track Initialize only position Lazy initialization Accumulate several unassociated observations Initialize position & velocity Two heuristics Greedy deletion Delete if no observation can be associated No occlusion handling Lazy deletion Delete if no observation can be associated for several time Implicit occlusion handling

69 Example: Tracking the Ball Unlike the previous experiment in which we had a model of the ball s trajectory and just observed it, we now want to track the ball Comparison: small versus large process noise Q and the effect of the three different motion models For simplicity, we perform no gaiting (i.e. no Mahalanobis test) but accept the pairing every time

70 Ball Tracking: Brownian Small process noise Large process noise Ground truth Observations State estimate

71 Ball Tracking: Brownian Ground truth Observations State estimate

72 Ball Tracking: Brownian Ground truth Observations State estimate

73 Ball Tracking: Brownian Ground truth Observations State estimate

74 Ball Tracking: Brownian Ground truth Observations State estimate

75 Ball Tracking: Brownian Ground truth Observations State estimate

76 Ball Tracking: Brownian Ground truth Observations State estimate

77 Ball Tracking: Brownian Ground truth Observations State estimate

78 Ball Tracking: Brownian Ground truth Observations State estimate

79 Ball Tracking: Brownian Ground truth Observations State estimate

80 Ball Tracking: Brownian Ground truth Observations State estimate

81 Ball Tracking: Brownian Ground truth Observations State estimate

82 Ball Tracking: Brownian Ground truth Observations State estimate

83 Ball Tracking: Brownian Ground truth Observations State estimate

84 Ball Tracking: Brownian large difference! Ground truth Observations State estimate

85 Ball Tracking: Constant Velocity Small process noise Large process noise Ground truth Observations State estimate

86 Ball Tracking: Constant Velocity Ground truth Observations State estimate

87 Ball Tracking: Constant Velocity Ground truth Observations State estimate

88 Ball Tracking: Constant Velocity Ground truth Observations State estimate

89 Ball Tracking: Constant Velocity Ground truth Observations State estimate

90 Ball Tracking: Constant Velocity Ground truth Observations State estimate

91 Ball Tracking: Constant Velocity Ground truth Observations State estimate

92 Ball Tracking: Constant Velocity Ground truth Observations State estimate

93 Ball Tracking: Constant Velocity Ground truth Observations State estimate

94 Ball Tracking: Constant Velocity Ground truth Observations State estimate

95 Ball Tracking: Constant Velocity Ground truth Observations State estimate

96 Ball Tracking: Constant Velocity Ground truth Observations State estimate

97 Ball Tracking: Constant Velocity Ground truth Observations State estimate

98 Ball Tracking: Constant Velocity Ground truth Observations State estimate

99 Ball Tracking: Constant Velocity smaller difference Ground truth Observations State estimate

100 Ball Tracking: Const. Acceleration Small process noise Large process noise Ground truth Observations State estimate

101 Ball Tracking: Const. Acceleration Ground truth Observations State estimate

102 Ball Tracking: Const. Acceleration Ground truth Observations State estimate

103 Ball Tracking: Const. Acceleration Ground truth Observations State estimate

104 Ball Tracking: Const. Acceleration Ground truth Observations State estimate

105 Ball Tracking: Const. Acceleration Ground truth Observations State estimate

106 Ball Tracking: Const. Acceleration Ground truth Observations State estimate

107 Ball Tracking: Const. Acceleration Ground truth Observations State estimate

108 Ball Tracking: Const. Acceleration Ground truth Observations State estimate

109 Ball Tracking: Const. Acceleration Ground truth Observations State estimate

110 Ball Tracking: Const. Acceleration Ground truth Observations State estimate

111 Ball Tracking: Const. Acceleration Ground truth Observations State estimate

112 Ball Tracking: Const. Acceleration Ground truth Observations State estimate

113 Ball Tracking: Const. Acceleration Ground truth Observations State estimate

114 Ball Tracking: Const. Acceleration very small difference! Ground truth Observations State estimate

115 Ball Tracking: Wrap Up The better the motion model, the better the tracker is able to follow the target A large process noise covariance can partly compensate a poor motion model But: large process noise covariances cause the validation gates to be large, which in turn, creates a high level of ambiguity for data association in case of multiple targets In other words: the tracker can t tell which is which target anymore because they are all statistically compatible with the observations