Multiple Sensor Target Tracking: Basic Idea Iterative updating of - PowerPoint PPT Presentation

Multiple Sensor Target Tracking: Basic Idea Iterative updating of conditional probability densities! kinematic target state x k at time t k , accumulated sensor data Z k a priori knowledge: target dynamics models, sensor model dynamics model p ( x k − 1 |Z k − 1 ) p ( x k |Z k − 1 ) • prediction: − − − − − − − − − − → sensor data Z k p ( x k |Z k − 1 ) p ( x k |Z k ) • filtering: − − − − − − − − − − → sensor model filtering output p ( x l − 1 |Z k ) p ( x l |Z k ) • retrodiction: ← − − − − − − − − − − dynamics model A first look at retrodiction today! Sensor Data Fusion - Methods and Applications, 4th Lecture on November 6, 2019 — slide 1

Recapitulation: An Important Data Fusion Algorithm k ) ⊤ , Z k = { z k , Z k − 1 } Kalman filter: x k = ( r ⊤ r ⊤ k , ˙ p ( x 0 ) = N � x 0 ; x 0 | 0 , P 0 | 0 � initiation: , initial ignorance: P 0 | 0 ‘large’ N � x k − 1 ; x k − 1 | k − 1 , P k − 1 | k − 1 � dynamics model N � x k ; x k | k − 1 , P k | k − 1 � prediction: − − − − − − − − − → F k | k − 1 , D k | k − 1 x k | k − 1 = F k | k − 1 x k − 1 | k − 1 ⊤ + D k | k − 1 P k | k − 1 = F k | k − 1 P k − 1 | k − 1 F k | k − 1 � � � � current measurement z k N − − − − − − − − − − − − − → N filtering: x k ; x k | k − 1 , P k | k − 1 x k ; x k | k , P k | k sensor model: H k , R k = x k | k − 1 + W k | k − 1 ν k | k − 1 , ν k | k − 1 = z k − H k x k | k − 1 x k | k S k | k − 1 = H k P k | k − 1 H k ⊤ + R k P k | k − 1 − W k | k − 1 S k | k − 1 W k | k − 1 ⊤ , = P k | k W k | k − 1 = P k | k − 1 H k ⊤ S k | k − 1 − 1 ‘K ALMAN gain matrix’ A deeper look into the dynamics and sensor models necessary! Sensor Data Fusion - Methods and Applications, 4th Lecture on November 6, 2019 — slide 2

Remember your own ground truth generator! Consider a car moving on a mountain pass road modeled by: � � � � vt x ( t ) a y sin( 4 πv Exercise 3.1 ax t ) r ( t ) = = y ( t ) a z sin( πv ax t ) z ( t ) v = 20 km h , a x = 10 km, a y = a z = 1 km, t ∈ [0 , a x /v ] . 1. Plot the trajectory. Are the parameters reasonable? Try alternatives. 2. Calculate and plot the velocity and acceleration vectors: � � � � x ( t ) ˙ x ( t ) ¨ r ( t ) = ˙ , ¨ r ( t ) = − q . y ( t ) ˙ y ( t ) ¨ z ( t ) ˙ z ( t ) ¨ 3. Calculate for each instance of time t the tangential vectors in r ( t ) : 1 t ( t ) = r ( t ) | ˙ r ( t ) . | ˙ 4. Plot | ˙ r ( t ) | , | ¨ r ( t ) | , and ¨ r ( t ) t ( t ) over the time interval. 5. Discuss the temporal behaviour based on the trajectory r ( t ) ! Sensor Data Fusion - Methods and Applications, 4th Lecture on November 6, 2019 — slide 3

Create your own sensor simulator! Simulate normally distributed radar measurements! ∆ T = 2 s, 2 radars at r 1 , 2 = ( x 1 , 2 s , y 1 , 2 , z 1 , 2 ) ⊤ , s s s Exercise 4.1 x 1 , 2 = 0, 100 km, y 1 , 2 = 100, 0 km, z 1 , 2 = 10 km. s s s State at time t k = k ∆ T , k ∈ Z : x k = ( r ⊤ r ⊤ k ) ⊤ k , ˙ 1. Simulate range and azimuth measurements of the target position r k with a random num- ber generator normrnd (0 , 1) producing normally distributed zero-mean and unit-variance random numbers: � √ � � � � � ( x k − x s ) 2 +( y k − y s ) 2 +( z k − z s ) 2 − z 2 z r z p σ r normrnd (0 , 1) k = = s + k z ϕ yk − ys σ ϕ normrnd (0 , 1) arctan( xk − xs ) k with σ r = 10 m, σ ϕ = 0 . 1 ◦ denoting the standard deviations in range and azimuth. As- sume that the radars are not able to measure the elevation angle (see discussion on the whiteboard!). k ) ⊤ + r s and k (cos z ϕ k , sin z ϕ 2. Transform the measurements in x - y -Cartesian coordinates z r plot them over x - y projection of the true target trajectory! Play with sensor positions and measurement error standard deviations! Sensor Data Fusion - Methods and Applications, 4th Lecture on November 6, 2019 — slide 4

Recapitulation: Piecewise Constant White Acceleration Consider state vectors: x k = ( r ⊤ r ⊤ k ) ⊤ k , ˙ (position, velocity) For known x k − 1 and without external influences we have with ∆ T k = t k − t k − 1 : � � � � I ∆ T k I r k − 1 x k = =: F k | k − 1 x k − 1 , see blackboard! O I r k − 1 ˙ Assume during the interval ∆ T k a constant acceleration a k causing the state evolution: � 1 � 2 ∆ T 2 k I a k =: G k a k , linear transform! ∆ T k I � � a k ; o , Σ 2 Let a k be a Gaussian RV with pdf: p ( a k ) = N k I , we therefore have: � � G k a k ; o , Σ 2 k G k G ⊤ p ( G k a k ) = N . k Sensor Data Fusion - Methods and Applications, 4th Lecture on November 6, 2019 — slide 5

� � p ( x k | x k − 1 ) = N x k ; F k | k − 1 x k − 1 , D k | k − 1 Therefore: with � � � 1 � 1 4 ∆ T 4 2 ∆ T 3 ∆ T k I k I k I I D k | k − 1 = Σ 2 F k | k − 1 = , 1 k 2 ∆ T 3 ∆ T 2 O I k I k I Sensor Data Fusion - Methods and Applications, 4th Lecture on November 6, 2019 — slide 6

Recapitulation Range, Azimuth Measurements • measurements in polar coordinates: � � σ 2 r 0 z k = ( r k , ϕ k ) ⊤ , measurement error: R = , r , ϕ independent 0 σ 2 ϕ • in Cartesian coord.: expand around truth position: � cos ϕ k � t [ z k ] = r k ≈ t [ r k | k − 1 ] + T ( z k − r k ) sin ϕ k � � � � − sin ϕ cos ϕ 1 0 T = sin ϕ cos ϕ 0 r � �� � � �� � rotation D ϕ dilation S r • Cartesian error covariance (time dependent): � � TRT ⊤ = D ϕ S r R S r D ⊤ σ 2 0 D ⊤ ϕ = D ϕ r ϕ 0 ( rσ ϕ ) 2 • sensor fusion: sensor-to-target-geometry enters into TRT ⊤ Sensor Data Fusion - Methods and Applications, 4th Lecture on November 6, 2019 — slide 7

� � p ( x 0 ) = N x 0 ; x 0 | 0 , P 0 | 0 initiation: , initial ignorance: P 0 | 0 ‘large’ � � dynamics model � � N x k − 1 ; x k − 1 | k − 1 , P k − 1 | k − 1 − − − − − − − − − → N x k ; x k | k − 1 , P k | k − 1 prediction: F k | k − 1 , D k | k − 1 x k | k − 1 = F k | k − 1 x k − 1 | k − 1 ⊤ + D k | k − 1 P k | k − 1 = F k | k − 1 P k − 1 | k − 1 F k | k − 1 N � x k ; x k | k − 1 , P k | k − 1 � � � current measurement z k filtering: − − − − − − − − − − − − − → N x k ; x k | k , P k | k sensor model: H k , R k ν k | k − 1 = z k − H k x k | k − 1 x k | k = x k | k − 1 + W k | k − 1 ν k | k − 1 , S k | k − 1 = H k P k | k − 1 H k ⊤ + R k P k | k − 1 − W k | k − 1 S k | k − 1 W k | k − 1 ⊤ , = P k | k W k | k − 1 = P k | k − 1 H k ⊤ S k | k − 1 − 1 ‘K ALMAN gain matrix’ In your sensor simulator, chose a sensor at position r s that produces x - y measurements z k of the Cartesian target x - y positions Hx k from your ground truth generator using the measurement matrix H : � 1 , 0 , 0 , 0 , 0 , 0 � Hx k = x k 0 , 1 , 0 , 0 , 0 , 0 Exercise 4.2 Calculate for each measurement the measurement error covariance matrix R k based on the true target position. Program your first Kalman filter initiated by the first measurement and reasonably chosen covariance matrices P 1 | 1 . What is reasonable? Visualize nicely and compare with the truth and the measurements. Sensor Data Fusion - Methods and Applications, 4th Lecture on November 6, 2019 — slide 8

Recapitulation: Create a singe effective measurement by preprocessing of the individual measurements! S k � − 1 z s � � R s z k = R k weighted arithmetic mean of measurements k k s =1 − 1   S k � − 1 � � R s R k = harmonic mean of measuremt covariances   k s =1 A typical structure for fusion equations! With measurement specific measurement error covariances, your Kalman filter already is a multiple Exercise 4.3 sensor fusion algorithms. Use two radar sensors, fuse the measurements, feed them into the Kalman filter and discuss the result! Sensor Data Fusion - Methods and Applications, 4th Lecture on November 6, 2019 — slide 9

More general: measurement process • linear measurement equation: � � z k = H k x k + u k , p ( u k ) = N u k ; o , R k – to be measured: linear functions of the object state – measurement error: biasfree, Gaussian distrib. independent for different t k – y k = z k − H k x k has the pdf: p ( y k ) = p ( u k ) • Approach for the requested pdf (‘likelihood fkt.): � � � � z k | x k = N p z k ; H k x k , R k • Example: position measurement H k = ( I , O , O ) , H k x k = r k R k : measurement error covariance matrix possibly depending on the sensor-to-target geometry Sensor Data Fusion - Methods and Applications, 4th Lecture on November 6, 2019 — slide 10

Retrodiction: How to calculate the pdf p ( x l |Z k ) ? Consider the past: l < k ! an observation: � � x l | Z k � � x l , x l +1 | Z k � p = d x l +1 p Sensor Data Fusion - Methods and Applications, 4th Lecture on November 6, 2019 — slide 11

Retrodiction: How to calculate the pdf p ( x l |Z k ) ? Consider the past: l < k ! an observation: � � � x l | Z k � � x l , x l +1 | Z k � � x l | x l +1 , Z k � � x l +1 | Z k � p = d x l +1 p = d x l +1 p p � �� � retrodiction: t l +1 Sensor Data Fusion - Methods and Applications, 4th Lecture on November 6, 2019 — slide 12