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_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 sample_MultiSensorGNN():
"""
This program calculate two target / two sensor case with GNN, and animate it.
"""
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
class Radar2DSensor(sensors.Polar2DSensor):
def __init__(self, **kwargs):
super().__init__(**kwargs)
def is_trk_in_range(self, trk):
dist, azim = cmath.polar( (trk.model.x[0]-self.x[0]) + 1j*(trk.model.x[1]-self.x[1]) )
return self.range_min < dist < self.range_max and abs(azim-self.angle) < self.width/2
def is_tgt_in_range(self, tgt):
dist, azim = cmath.polar( (tgt.x[0]-self.x[0]) + 1j*(tgt.x[1]-self.x[1]) )
return self.range_min < dist < self.range_max and abs(azim-self.angle) < self.width/2
sen_list = [
Radar2DSensor(
PD=PD,
VC=1.0,
PFA=PFA,
BNT=0.03,
x0=np.array([0.0, 10.0, 1.0, 0.0]),
angle0=15/180*cmath.pi,
DETECT_RANGE_MAX=40,
DETECT_RANGE_MIN=10,
DETECT_WIDTH=120/180*cmath.pi
),
Radar2DSensor(
PD=PD,
VC=1.0,
PFA=PFA,
BNT=0.03,
x0=np.array([50.0, 10.0, 1.0, 0.0]),
angle0=-15/180*cmath.pi,
DETECT_RANGE_MAX=40,
DETECT_RANGE_MIN=10,
DETECT_WIDTH=120/180*cmath.pi
)
]
tgt_list = [
models.SimpleTarget(SD=2, x0=[100., 00.,-1.,+0.3]),
models.SimpleTarget(SD=2, x0=[100., 10.,-1.,-0.1]),
models.SimpleTarget(SD=2, x0=[100., 20.,-1.,-0.2]),
]
tracker = trackers.GNN(
sen_list=sen_list,
model_factory=models.SingerModelFactory(
model=models.KalmanModel,
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.ScoreManagedTrack
)
)
obs_df = pd.DataFrame()
tgt_df = pd.DataFrame()
trk_df = pd.DataFrame()
sen_df = pd.DataFrame()
for i_scan in range(100):
timestamp = pd.Timestamp(i_scan, unit="s")
if not i_scan % 5:
# scan by sensor0 (once in 5 times)
sensor = sen_list[0]
R = np.eye(2) * 0.01
obs_list = [models.Obs(np.random.multivariate_normal(tgt.x[:2], R), R) for tgt in tgt_list if sensor.is_tgt_in_range(tgt)]
trk_list = tracker.register_scan(obs_list, sensor=sensor)
else:
# scan by sensor1 (everytime except sensor0 turn)
sensor = sen_list[1]
R = np.eye(2) * 0.01
obs_list = [models.Obs(np.random.multivariate_normal(tgt.x[:2], R), R) for tgt in tgt_list if sensor.is_tgt_in_range(tgt)]
trk_list = tracker.register_scan(obs_list, sensor=sensor)
# tgt_list update
[ tgt.update(tracker._dT()) for tgt in tgt_list ]
# save as dataframe
obs_df = obs_df.append( [ obs.to_record(timestamp, i_scan, tracker.y_mdl_type()) for obs in obs_list ], ignore_index=True )
trk_df = trk_df.append( [ trk.to_record(timestamp, i_scan) for trk in trk_list ], ignore_index=True )
tgt_df = tgt_df.append( [ tgt.to_record(timestamp, i_scan) for tgt in tgt_list ], ignore_index=True)
sen_df = sen_df.append( [ sen.to_record(timestamp, i_scan) for sen in sen_list ], ignore_index=True )
# export
anal = analyzers.BaseAnalyzer.import_df(obs_df, trk_df, sen_df, tgt_df)
anal.export_csv()
anal.export_db()
# analyse
anal.animation()
def sample_FGT(is_fgt=True):
PD = 0.75
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
np.random.seed(0)
class Radar2DSensor(sensors.Polar2DSensor):
def __init__(self, **kwargs):
super().__init__(**kwargs)
def is_trk_in_range(self, trk):
dist, azim = cmath.polar( (trk.model.x[0]-self.x[0]) + 1j*(trk.model.x[1]-self.x[1]) )
return self.range_min < dist < self.range_max and abs(azim-self.angle) < self.width/2
def is_tgt_in_range(self, tgt):
dist, azim = cmath.polar( (tgt.x[0]-self.x[0]) + 1j*(tgt.x[1]-self.x[1]) )
return self.range_min < dist < self.range_max and abs(azim-self.angle) < self.width/2
sen_list = [
Radar2DSensor(
PD=PD,
VC=1.0,
PFA=PFA,
BNT=0.03,
x0=np.array([0.0, 10.0, 0.0, 0.0]),
angle0=0.0/180*cmath.pi,
DETECT_RANGE_MAX=10000,
DETECT_RANGE_MIN=10,
DETECT_WIDTH=120/180*cmath.pi
)
]
tgt_list = [
models.SimpleTarget(SD=2, x0=[10000., 00.,-10.,+0.0]),
models.SimpleTarget(SD=2, x0=[10000., 10.,-10.,+0.0]),
models.SimpleTarget(SD=2, x0=[10000., 20.,-10.,-0.0]),
models.SimpleTarget(SD=2, x0=[10000., 30.,-10.,-0.0]),
models.SimpleTarget(SD=2, x0=[10000., 40.,-10.,-0.0]),
models.SimpleTarget(SD=2, x0=[10000., 50.,-10.,-0.0]),
models.SimpleTarget(SD=2, x0=[10000., 60.,-10.,-0.0]),
models.SimpleTarget(SD=2, x0=[10000., 70.,-10.,-0.0]),
models.SimpleTarget(SD=2, x0=[10000., 80.,-10.,-0.0]),
]
if is_fgt:
tracker = trackers.FGT(
sen_list=sen_list,
model_factory=models.SingerModelFactory(
model=models.KalmanModel,
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.FormationTrack
)
)
else:
tracker = trackers.GNN(
sen_list=sen_list,
model_factory=models.SingerModelFactory(
model=models.KalmanModel,
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.SimpleManagedTrack
)
)
obs_df = pd.DataFrame()
tgt_df = pd.DataFrame()
trk_df = pd.DataFrame()
sen_df = pd.DataFrame()
for i_scan in range(100):
timestamp = pd.Timestamp(i_scan, unit="s")
sensor = sen_list[0]
R = np.eye(2) * 20.0
obs_list = [
models.Obs(np.random.multivariate_normal(tgt.x[:2], R), R)for tgt in tgt_list
if sensor.is_tgt_in_range(tgt) and np.random.choice([True, False], p=[PD, 1-PD])
]
trk_list = tracker.register_scan(obs_list, sensor=sensor)
# tgt_list update
[ tgt.update(tracker._dT()) for tgt in tgt_list ]
# save as dataframe
obs_df = obs_df.append( [ obs.to_record(timestamp, i_scan, tracker.y_mdl_type()) for obs in obs_list ], ignore_index=True )
trk_df = trk_df.append( [ trk.to_record(timestamp, i_scan) for trk in trk_list ], ignore_index=True )
tgt_df = tgt_df.append( [ tgt.to_record(timestamp, i_scan) for tgt in tgt_list ], ignore_index=True)
sen_df = sen_df.append( [ sen.to_record(timestamp, i_scan) for sen in sen_list ], ignore_index=True )
# export
anal = analyzers.BaseAnalyzer.import_df(obs_df, trk_df, sen_df, tgt_df)
if is_fgt:
anal.export_db(fpath="data_fgt")
else:
anal.export_db(fpath="data_gnn")
# analyze
anal.plot2D()
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_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)