Create your own ground truth generator! Consider a car moving on a - - PowerPoint PPT Presentation

Create your own ground truth generator!

Exercise 3.1

Consider a car moving on a mountain pass road modeled by:

r(t) =

• x(t)

y(t) z(t)

• =
• vt

ay sin( 4πv

ax t)

az sin( πv

axt)

• v = 20km

h , ax = 10 km, ay = az = 1 km, t ∈ [0, ax/v].

• 1. Plot the trajectory. Are the parameters reasonable? Try alternatives.
• 2. Calculate and plot the velocity and acceleration vectors:

˙

r(t) =

• ˙

x(t) ˙ y(t) ˙ z(t)

• ,

¨

r(t) =

• ¨

x(t) ¨ y(t) ¨ z(t)

• .
• 3. Calculate for each instance of time t the tangential vectors in r(t):

t(t) =

1 |˙ 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, Programming Exercises, WS 2019-20 — slide 1

Create your own sensor simulator!

Exercise 4.1

Simulate normally distributed radar measurements! ∆T = 2 s, 2 radars at r1,2

s

= (x1,2

s , y1,2 s

, z1,2

s

)⊤, x1,2

s

= 0, 100 km, y1,2

s

= 100, 0 km, z1,2

s

= 10 km. State at time tk = k∆T, k ∈ Z: xk = (r⊤

k , ˙

r⊤

k )⊤

• 1. Simulate range and azimuth measurements of the target position rk with a random num-

ber generator normrnd(0, 1) producing normally distributed zero-mean and unit-variance random numbers:

zp

k =

• zr

k

zϕ

k

• =

√

(xk−xs)2+(yk−ys)2+(zk−zs)2−z2

s

arctan(

yk−ys xk−xs)

• +
• σr normrnd(0,1)

σϕ normrnd(0,1)

• 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!).

• 2. Transform the measurements in x-y-Cartesian coordinates zr

k(cos zϕ k , sin zϕ k )⊤ + rs and

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, Programming Exercises, WS 2019-20 — slide 2

initiation: p(x0) = N

• x0; x0|0, P0|0
• ,

initial ignorance:

P0|0 ‘large’

prediction: N

• xk−1; xk−1|k−1, Pk−1|k−1
• dynamics model

− − − − − − − − − →

Fk|k−1, Dk|k−1

N

• xk; xk|k−1, Pk|k−1
• xk|k−1 = Fk|k−1xk−1|k−1

Pk|k−1 = Fk|k−1Pk−1|k−1Fk|k−1

⊤ + Dk|k−1

filtering: Nxk; xk|k−1, Pk|k−1

• current measurement zk

− − − − − − − − − − − − − →

sensor model: Hk, Rk

N

• xk; xk|k, Pk|k
• xk|k

=

xk|k−1 + Wk|k−1νk|k−1, νk|k−1 = zk − Hkxk|k−1 Pk|k

=

Pk|k−1 − Wk|k−1Sk|k−1Wk|k−1⊤, Sk|k−1 = HkPk|k−1Hk⊤ + Rk Wk|k−1 = Pk|k−1Hk⊤Sk|k−1−1

‘KALMAN gain matrix’

Exercise 4.2

In your sensor simulator, chose a sensor at position rs that produces x-y measurements zk of the Cartesian target x-y positions Hxk from your ground truth generator using the measurement matrix H:

Hxk = 1,0,0,0,0,0

0,1,0,0,0,0

• xk

Calculate for each measurement the measurement error covariance matrix Rk based on the true target position. Program your first Kalman filter initiated by the first measurement and reasonably chosen covariance matrices P1|1. What is reasonable? Visualize nicely and compare with the truth and the measurements.

Sensor Data Fusion - Methods and Applications, Programming Exercises, WS 2019-20 — slide 3

Selected exercises to be chosen from:

Retrodiction — discrete time, eventually continuous time. Simulate missing and false plots — Realize a PDAF tracker. Consider useful IMM parameters — two model IMM tracker. Discretize the road — Realize a road map assisted tracker. Create a GMTI sensor simulator — Realize a GMTI tracker. Test the consistency of one of your trackers — NIS, NEES.

Sensor Data Fusion - Methods and Applications, Programming Exercises, WS 2019-20 — slide 4