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)