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_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 create_obs_list(self, tgt_list, R=None, PD=None, PFA=None):
assert "R" in self.param
assert "PD" in self.param
assert "PFA" in self.param
assert "y_mins" in self.param
assert "y_maxs" in self.param
assert "y_stps" in self.param
if R is None:
R = self.param["R"]
if PD is None:
PD = self.param["PD"]
if PFA is None:
PFA = self.param["PFA"]
y_mins = self.param["y_mins"]
y_maxs = self.param["y_maxs"]
y_stps = self.param["y_stps"]
n_mesh = int(
np.prod([
abs((y_max - y_min) / y_stp)
for y_min, y_max, y_stp in zip(y_mins, y_maxs, y_stps)
]))
# init observation
obs_list = []
# add target observation
obs_list.extend([
models.Obs(
np.random.multivariate_normal(tgt.x[:len(R)],
R), # set as real parameter R
self.param["R"] # set as model parameter R
) for tgt in tgt_list if tgt.is_exist()
and np.random.choice([True, False], p=[PD, 1 - PD])
])
# add false alarm observation
obs_list.extend([
models.Obs(
np.array([
np.random.uniform(y_min, y_max)
for y_min, y_max in zip(y_mins, y_maxs)
]),
self.param["R"] # set as model parameter R
) for k in range(stats.binom.rvs(n=n_mesh, p=PFA))
])
return obs_list
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 _initialize_simulation(self):
self.tgt_list = []
self.obs_list = []
self.trk_list = []
tgt = self.target
obs = models.Obs(
np.random.multivariate_normal(tgt.x[:len(self.R)], self.R), self.R,
self.sensor)
trk = self.track_factory.create(obs)
assert tgt.x.shape == trk.model.x.shape, "target.x.shape and model.x.shape invalid, actual:" + str(
tgt.x.shape) + str(trk.model.x.shape)
return (tgt, obs, trk)
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_LinearKalmanModel(self):
"""Linear Kalman Filter
ref) Design and Analysis of Modern Tracking Systems
3.3.3 Example of a Two-State Kalman Filter
"""
PD = 0.5
sig_dv = 1
sig_x0 = 10
sig_vx0 = 5
sig_o = 5
T = 1
x = np.array([0, 0])
F = np.array([[1, T], [0, 1]])
H = np.array([[1, 0]])
P = np.array([[sig_x0**2, 0], [0, sig_vx0**2]])
Q = np.array([[0, 0], [0, sig_dv**2]])
model = models.KalmanModel(x, F, H, P, Q, False)
mdl_list = []
for y in np.random.normal(loc=0, scale=sig_o, size=36):
if np.random.choice([True, False], p=[PD, 1 - PD]):
model.update(models.Obs(y, np.eye(1) * sig_o**2))
mdl_list.append(copy.deepcopy(model))
else:
model.update(None)
mdl_list.append(None)
if False:
plt.plot(
[i for i in range(len(mdl_list))],
[mdl.K[0] if mdl is not None else None for mdl in mdl_list],
marker="D",
color="g",
alpha=.5,
linestyle="None")
plt.plot(
[i for i in range(len(mdl_list))],
[mdl.K[1] if mdl is not None else None for mdl in mdl_list],
marker="D",
color="r",
alpha=.5,
linestyle="None")
plt.show()
def _update(self, tgt, obs, trk):
# update target
tgt.update(self._dT())
# update observation
obs = models.Obs(
np.random.multivariate_normal(tgt.x[:len(self.R)], self.R), self.R,
self.sensor)
dist, detS, M = trk.model.norm_of_residual(obs)
gate = obs.sensor.calc_ellipsoidal_gate(detS, M)
mch_scr = trk._calc_match_score(obs)
ini_scr = trk._calc_init_score(obs)
# track update
trk.assign(obs)
return (tgt, obs, trk,
dict(dist=dist, gate=gate, mch_scr=mch_scr, ini_scr=ini_scr))
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_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 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)
