def test_ModelEvaluator(self):
eval = models.ModelEvaluator(sensor=sensors.BaseSensor(),
model_factory=models.SingerModelFactory(
models.KalmanModel,
dT=1.0,
tm=10.0,
sm=15.0,
SD=1,
is_vel_measure_enabled=False),
target=models.SingerTarget(tm=10.0,
sm=15.0,
SD=1),
R=np.diag([150**2]))
RMSE = eval.estimate_prediction_error()
np.testing.assert_almost_equal(RMSE.shape, (3, ))
eval = models.ModelEvaluator(sensor=sensors.BaseSensor(),
model_factory=models.SingerModelFactory(
models.KalmanModel,
dT=1.0,
tm=10.0,
sm=15.0,
SD=1,
is_vel_measure_enabled=True),
target=models.SingerTarget(tm=10.0,
sm=15.0,
SD=1),
R=np.diag([150**2, 5.**2]))
RMSE = eval.estimate_prediction_error()
np.testing.assert_almost_equal(RMSE.shape, (3, ))
def test_FormationTrack(self):
PD=0.7
PFD=1.e-3
NFC=4
NFA=1000
model_factory = models.SimpleModelFactory(
model = models.KalmanModel,
dT=1.0,
q=0.001
)
sensor = sensors.BaseSensor(
PD=PD,
VC=1.0,
PFA=1e-6,
BNT=0.03
)
# assign 1 track
obs = models.Obs(np.array([1.0, 2.0]), np.eye(2), sensor )
trk = tracks.FormationTrack(obs,model_factory.create(obs))
# assign 2 track
obs = models.Obs(np.array([3.0, 4.0]), np.eye(2), sensor )
trk = tracks.FormationTrack(obs,model_factory.create(obs))
def test_TrackerEvaluator(self):
eval = trackers.TrackerEvaluator(
tracker=trackers.GNN(sensor=sensors.BaseSensor(R=np.diag(
[0.001, 0.001]),
PD=0.7,
VC=1.0,
PFA=1e-6,
BNT=0.03,
y_mins=[-250, -250],
y_maxs=[+250, +250],
y_stps=[1, 1]),
model_factory=models.SimpleModelFactory(
model=models.KalmanModel, dT=1.0, q=1.0),
track_factory=tracks.BaseTrackFactory(
track=tracks.ScoreManagedTrack)),
tgt_list=[
models.SimpleTarget(SD=2,
x0=[0., 0., +0., +1.],
start_time=0.0),
models.SimpleTarget(SD=2,
x0=[0., 10., +1., -1.],
start_time=0.0),
models.SimpleTarget(SD=2,
x0=[10., 10., -1., -1.],
start_time=0.0),
models.SimpleTarget(SD=2,
x0=[10., 0., +0., +1.],
start_time=1.0)
])
# eval.plot_position()
eval.estimate_track_statistics()
def test_SimpleManagedTrack(self):
ND=3
tracker = MockTracker(
sensor=sensors.BaseSensor(
PD=0.7,
VC=1.0,
PFA=1e-6,
BNT=0.03
),
model_factory=models.SimpleModelFactory(
model=models.KalmanModel,
dT=1.0,
q=0.001
)
)
tgt = np.array([0.0, 0.0, 1.0, 1.0])
for k in range(200):
tracker.count = k
if k==0:
obs = models.Obs(tgt[:2], np.eye(2), tracker.sensor )
trk = tracks.SimpleManagedTrack(
obs,
tracker.model_factory.create(obs),
ND=ND
)
if 0 < k <= 100:
tgt[0:2] += tgt[2:]
trk.assign(models.Obs(
# tgt[:2], np.eye(2), tracker.sensor
np.random.multivariate_normal(tgt[:2], np.eye(2)), np.eye(2), tracker.sensor
))
if 100 < k:
trk.unassign(tracker.sensor)
# judge_deletion test
if 0 <= k < 100+ND:
np.testing.assert_equal(trk.judge_deletion(), False)
pass
elif 100+ND <= k:
np.testing.assert_equal(trk.judge_deletion(), True)
pass
# after loop
if False:
# trk.plot_obs_list()
# trk.plot_mdl_list()
plt.show()
def test_SimpleModelFactory(self):
# to_record/from_record check
mf = models.SimpleModelFactory(model=models.KalmanModel, dT=1.0, q=1.0)
mdl1 = mf.create(
models.Obs(y=np.array([1, 0]),
R=np.zeros((2, 2)) + 0.1,
sensor=sensors.BaseSensor()))
series = mdl1.to_record()
mdl2 = mf.create_from_record(series)
def test_TrackEvaluator(self):
scan_time = 1.0
sigma_o = 1.0
time_m = 2.0
sigma_mx = 4.0
sigma_my = 1.0
sigma_vx = 18.0
sigma_vy = 4.0
vx0 = np.random.normal(0.0, sigma_vx)
vy0 = np.random.normal(0.0, sigma_vy)
eval = tracks.TrackEvaluator(
sensor=sensors.BaseSensor(
PD=0.7,
VC=1.0,
PFA=1e-6,
BNT=0.03
),
model_factory=models.SingerModelFactory(
model=models.KalmanModel,
dT=1.0,
tm=time_m,
sm=[sigma_mx, sigma_my],
SD=2,
P0=np.diag([sigma_o**2, sigma_o**2, sigma_vx**2, sigma_vy**2])
),
track_factory=tracks.BaseTrackFactory(
track=tracks.ScoreManagedTrack
),
target=models.SimpleTarget(
x0=[0.0, 0.0, vx0, vy0],
SD=2
),
R=np.diag([sigma_o**2, sigma_o**2])
)
# eval.plot_score()
def test_GNN(self):
tracker = trackers.GNN(sensor=sensors.BaseSensor(PD=0.7,
VC=1.0,
PFA=1e-6,
BNT=0.03),
model_factory=models.SimpleModelFactory(
model=models.KalmanModel, dT=1.0, q=0.001),
track_factory=tracks.BaseTrackFactory(
track=tracks.ScoreManagedTrack))
tgt_list = [
np.array([-1, 1]),
np.array([20, 36]),
]
tracker.register_scan([models.Obs(y, np.eye(2) * 5) for y in tgt_list])
tgt_list = [
np.array([0, 0]),
np.array([19, 37]),
np.array([40, 50]),
]
tracker.register_scan([models.Obs(y, np.eye(2) * 5) for y in tgt_list])
def test_ScoreManagedTrack(self):
"""Track Score Function
ref) Design and Analysis of Modern Tracking Systems
6.2 Track Score Function
"""
PD=0.7
PFD=1.e-3
NFC=4
NFA=1000
tracker = MockTracker(
sensor=sensors.BaseSensor(
PD=PD,
VC=1.0,
PFA=1e-6,
BNT=0.03
),
model_factory=models.SimpleModelFactory(
model=models.KalmanModel,
dT=1.0,
q=0.001
)
)
tgt = np.array([0.0, 0.0, 1.0, 1.0])
com_list = []
del_list = []
for k in range(200):
tracker.count = k
if k==0:
obs = models.Obs(tgt[:2], np.eye(2), tracker.sensor )
trk = tracks.ScoreManagedTrack(
obs,
tracker.model_factory.create(obs),
PFD=PFD,
alpha=NFC/3600/NFA,
beta=0.1
)
if 0 < k <= 100:
tgt[0:2] += tgt[2:]
obs = models.Obs(
# tgt[:2], np.eye(2), tracker.sensor
np.random.multivariate_normal(tgt[:2], np.eye(2)), np.eye(2), tracker.sensor
)
if np.random.choice([True, False], p=[PD, 1-PD]):
trk.assign(obs)
else:
trk.unassign(tracker.sensor)
if 100 < k:
trk.unassign(tracker.sensor)
com_list.append(trk.judge_confirmation())
del_list.append(trk.judge_deletion())
# after loop
if False:
# trk.plot_obs_list()
# trk.plot_mdl_list()
# trk.plot_scr_list()
plt.plot([i for i in range(200)], np.array(com_list, int) - np.array(del_list, int))
plt.show()
def sample_JPDA():
"""
This program calculate two target case with JPDA, and animate it.
But there are some problems that I still not solved.
1. Track Confirmation and Deletion
In this case, I implemented IPDA method for track confirmation and deletion,
but the judgement argorithm is alternative one I made due to lack of knowledge.
Temporally, I set the threshold of Pt (deletion at under 0.4, confirmation at over 0.95).
However it seems not to be good. It's needed to search more literature about IPDA.
2. Presentation Logic
JPDA has many unlikely tracks, so it should be to implement presentation logic, which
select likely track and display them. But not implemented yet.
"""
PD = 0.99
PFA = 1e-7
scan_time = 0.5
sigma_o = 1.0
time_m = 2.0
sigma_mx = 4.0
sigma_my = 1.0
sigma_vx = 18.0
sigma_vy = 4.0
tracker = trackers.JPDA(
sensor=sensors.BaseSensor(
PD=PD,
VC=1.0,
PFA=PFA,
BNT=0.03
),
model_factory=models.SingerModelFactory(
model=models.PDAKalmanModel,
dT=scan_time,
tm=time_m,
sm=[sigma_mx, sigma_my],
SD=2,
P0=np.diag([sigma_o**2, sigma_o**2, sigma_vx**2, sigma_vy**2])
),
track_factory=tracks.BaseTrackFactory(
track=tracks.PDATrack
)
)
tgt_list = [
np.array([0.,0.,1.,1.]),
np.array([100.,100.,-1.,-1.])
]
art_list =[]
fig = plt.figure()
plt.axis("equal")
plt.grid()
for i_scan in range(10):
trk_list = tracker.register_scan(
[models.Obs(tgt[:2], np.eye(2) * 5) for tgt in tgt_list]
)
tgt_art = plt.plot(
[tgt[0] for tgt in tgt_list ],
[tgt[1] for tgt in tgt_list ],
marker="D", color="b", alpha=.5, linestyle="None", label="tgt"
)
trk_art = plt.plot(
[trk.model.x[0] if trk is not None else None for trk in trk_list ],
[trk.model.x[1] if trk is not None else None for trk in trk_list ],
marker="D", color="r", alpha=.5, linestyle="None", label="trk"
)
ax_pos = plt.gca().get_position()
count = fig.text( ax_pos.x1-0.1, ax_pos.y1-0.05, "count:" + str(i_scan), size = 10 )
art_list.append( trk_art + tgt_art + [count] )
for tgt in tgt_list:
tgt[:2] += tgt[2:]*scan_time
_ = ani.ArtistAnimation(fig, art_list, interval=1000)
plt.show()
def generate_irst_example_p878(PD=0.7, PFA=1e-6, tracker_type="GNN"):
""" IRST example of p.878
unit is pixcel (=70urad)
ref) Design and Analysis of Modern Tracking Systems
13.3.5 IRST Example
"""
scan_time = 1.0
sigma_o = 1.0
time_m = 2.0
sigma_mx = 4.0
sigma_my = 1.0
sigma_vx = 18.0
sigma_vy = 4.0
vx0 = np.random.normal(0.0, sigma_vx)
vy0 = np.random.normal(0.0, sigma_vy)
if tracker_type == "JPDA":
tracker = trackers.JPDA
model = models.PDAKalmanModel
track = tracks.PDATrack
else:
tracker = trackers.GNN
model = models.KalmanModel
track = tracks.ScoreManagedTrack
ret = trackers.TrackerEvaluator(
tracker=tracker(
sensor=sensors.BaseSensor(
R=np.diag([sigma_o**2, sigma_o**2]),
PD=PD,
VC=1.0,
PFA=PFA,
BNT=0.03,
# y_mins=[-44880,-250],
# y_maxs=[+44880,+250],
y_mins=[-1125, -250], # reduce calculation load
y_maxs=[+1125, +250], # reduce calculation load
y_stps=[1, 1]),
model_factory=models.SingerModelFactory(
model=model,
dT=scan_time,
tm=time_m,
sm=[sigma_mx, sigma_my],
SD=2,
P0=np.diag([sigma_o**2, sigma_o**2, sigma_vx**2,
sigma_vy**2])),
track_factory=tracks.BaseTrackFactory(track=track)),
tgt_list=[
models.SimpleTarget(x0=[0.0, 0.0, vx0, vy0], SD=2),
],
R=np.diag([sigma_o**2, sigma_o**2]))
# TODO: implement SPRT, SMC(MHT), FMC(MHT), FMC(IPDA)
# SPRT( Sequential Probability Ratio Test )
# 13.3.3 SPRT Analysis of Track Confirmation (p874)
# SMC ( Simplified Monte Carlo Simulation )
# 13.3.4 Simplified Monte Carlo Simulation (p877)
# FMC ( Full Monte Carlo Simulation ) (p878)
# 13.3.5 IRST Example
return ret
def generate_irst_example_p372(PD=0.7,
PFA=1e-6,
is_maneuver_enabled=True,
tracker_type="GNN"):
""" IRST example of p.372
unit is pixcel (=70urad)
ref) Design and Analysis of Modern Tracking Systems
6.8.2 Simulation Study Results
"""
scan_time = 1.0
sigma_o = 1.0
time_m = 2.5
if is_maneuver_enabled:
target = SinusoidTarget(A=9.0, Tp=3.5)
sigma_mx = 25.0
sigma_my = 2.0
else:
target = SinusoidTarget(A=0.0, Tp=3.5)
sigma_mx = 5.0
sigma_my = 2.0
if tracker_type == "JPDA":
tracker = trackers.JPDA
model = models.PDAKalmanModel
track = tracks.PDATrack
else:
tracker = trackers.GNN
model = models.KalmanModel
track = tracks.ScoreManagedTrack
ret = TrackerEvaluatorForP372(
tracker=tracker(
sensor=sensors.BaseSensor(
R=np.diag([sigma_o**2, sigma_o**2]),
PD=PD,
VC=1.0,
PFA=PFA,
BNT=0.03,
# y_mins=[-44880,-250],
# y_maxs=[+44880,+250],
y_mins=[-50, -50], # reduce calculation load
y_maxs=[+50, +50], # reduce calculation load
y_stps=[1, 1]),
model_factory=models.SingerModelFactory(
model=model,
dT=scan_time,
tm=time_m,
sm=[sigma_mx, sigma_my],
SD=2,
P0=np.diag([
sigma_o**2, sigma_o**2, (target.A * target.omega)**2,
(target.A * target.omega)**2
])),
track_factory=tracks.BaseTrackFactory(track=track)),
tgt_list=[target],
R=np.diag([sigma_o**2, sigma_o**2]),
PD=PD,
PFA=PFA)
return ret
def test_JPDA(self):
"""Calculate Association of Observations by JPDA Method
ref) Design and Analysis of Modern Tracking Systems
6.6.2 Extension to JPDA
"""
tracker = trackers.JPDA(
sensor=sensors.BaseSensor(PD=0.7, VC=1.0, PFA=1e-6, BNT=0.03),
model_factory=models.SimpleModelFactory(
model=models.PDAKalmanModel, dT=1.0, q=0.0),
track_factory=tracks.BaseTrackFactory(track=tracks.PDATrack,
gate=8))
a = 2
x1 = (a**2 + 2 - 2.5) / 2 / a
x2 = (a**2 + 4 - 3) / 2 / a
obs_list = [
models.Obs(y,
np.eye(2) * 0.5) for y in [
np.array([0, 0]),
np.array([a, 0]),
]
]
tracker.register_scan(obs_list)
obs_list = [
models.Obs(y,
np.eye(2) * 0.5) for y in [
np.array([-1, 0]),
np.array([x1, np.sqrt(2 - x1**2)]),
np.array([x2, np.sqrt(4 - x2**2)]),
]
]
expected = np.array([1.0, 9.0, 2.0, 2.5, 4.0, 3.0])
actual = np.array([
dy @ np.linalg.inv(S) @ dy for dy, S in [
trk.model.residual(obs) for obs in obs_list
for trk in tracker.trk_list
]
])
np.testing.assert_almost_equal(actual, expected)
tracker.register_scan(obs_list)
expected = np.array([
6.47e-5,
5.04e-5,
3.06e-5,
1.44e-5,
1.82e-5,
1.11e-5,
8.60e-6,
6.70e-6,
4.10e-6,
2.40e-6,
])
actual = tracker.hyp_price_list
np.testing.assert_almost_equal(np.sort(actual), np.sort(expected))
def test_SignerModelFactory(self):
"""Singer Model Linear Kalman Filter
ref) Design and Analysis of Modern Tracking Systems
4.2.1 Singer Acceleration Model
"""
# simple check
mf = models.SingerModelFactory(model=models.KalmanModel,
dT=1.0,
tm=1.0,
sm=1.0,
SD=1)
md = mf.create(
models.Obs(y=np.array([1.0]),
R=np.zeros((1, 1)) + 0.1,
sensor=sensors.BaseSensor()))
np.testing.assert_equal(md.x.shape, (3, ))
np.testing.assert_equal(md.F.shape, (3, 3))
np.testing.assert_equal(md.H.shape, (1, 3))
np.testing.assert_equal(md.P.shape, (3, 3))
np.testing.assert_equal(md.Q.shape, (3, 3))
expected = [1.0, 0.0, 0.0]
np.testing.assert_almost_equal(md.x, expected)
expected = [[1, 1, np.exp(-1)], [0, 1, 1 - np.exp(-1)],
[0, 0, np.exp(-1)]]
np.testing.assert_almost_equal(md.F, expected)
expected = [[1, 0, 0]]
np.testing.assert_almost_equal(md.H, expected)
q11 = 1 - np.exp(-2) + 2 + 2 / 3 - 2 - 4 * np.exp(-1)
q12 = np.exp(-2) + 1 - 2 * np.exp(-1) + 2 * np.exp(-1) - 2 + 1
q13 = 1 - np.exp(-2) - 2 * np.exp(-1)
q22 = 4 * np.exp(-1) - 3 - np.exp(-2) + 2
q23 = np.exp(-2) + 1 - 2 * np.exp(-1)
q33 = 1 - np.exp(-2)
expected = [[q11, q12, q13], [q12, q22, q23], [q13, q23, q33]]
np.testing.assert_almost_equal(md.Q, expected)
# check @ beta*dT -> 0
sm = 7
tm = 1.e+3
dT = 2
mf = models.SingerModelFactory(model=models.KalmanModel,
dT=dT,
tm=tm,
sm=sm,
SD=1)
md = mf.create(
models.Obs(y=np.array([1.0]),
R=np.zeros((1, 1)) + 0.1,
sensor=sensors.BaseSensor()))
np.testing.assert_equal(md.x.shape, (3, ))
np.testing.assert_equal(md.F.shape, (3, 3))
np.testing.assert_equal(md.H.shape, (1, 3))
np.testing.assert_equal(md.P.shape, (3, 3))
np.testing.assert_equal(md.Q.shape, (3, 3))
expected = [1.0, 0.0, 0.0]
np.testing.assert_almost_equal(md.x, expected)
expected = [[1, dT, dT**2 / 2], [0, 1., dT], [0, 0., 1]]
np.testing.assert_almost_equal(md.F, expected, decimal=2)
expected = [[1, 0, 0]]
np.testing.assert_almost_equal(md.H, expected)
expected = np.array([[dT**5 / 20, dT**4 / 8, dT**3 / 6],
[dT**4 / 8, dT**3 / 3, dT**2 / 2],
[dT**3 / 6, dT**2 / 2, dT]])
np.testing.assert_almost_equal(md.Q,
expected * 2 * sm**2 / tm,
decimal=2)