def setUp(self):
self.radar1 = Radar(x=800,y=800)
self.radar2 = Radar(x=200,y=200)
radars = [self.radar1,self.radar2]
self.q = 10.
self.multiplef_cv = MultiplePeriodRadarsFilterCV(dim_x = 9, dim_z = 3, q = self.q,
radars = radars,
x0 = 100, y0 = 100)
def setUp(self):
# Radar & States generation
self.setUp_radar_states()
# Filter definition
## Classical models
self.radar_filter_ca = MultiplePeriodRadarsFilterCA(dim_x=9,
dim_z=3,
q=100.,
radars=self.radars)
self.radar_filter_cv = MultiplePeriodRadarsFilterCV(dim_x=9,
dim_z=3,
q=100.,
radars=self.radars)
self.radar_filter_ct = MultiplePeriodRadarsFilterCT(dim_x=9,
dim_z=3,
q=100.,
radars=self.radars)
## IMM with ca, cv and ct models
filters = [
self.radar_filter_cv, self.radar_filter_ca, self.radar_filter_ct
]
mu = [0.33, 0.33, 0.33]
trans = np.array([[0.998, 0.001, 0.001], [0.050, 0.900, 0.050],
[0.001, 0.001, 0.998]])
self.radar_filter = RadarIMM(filters=filters, mu=mu, M=trans)
# Benchmark definition
self.benchmark = Benchmark(radars=self.radars,
radar_filter=self.radar_filter,
states=self.states)
def setUp(self):
# Radar & States generation
self.setUp_radar_states()
# Filter definition: CA model
self.radar_filter = MultiplePeriodRadarsFilterCV(dim_x=9,
dim_z=6,
q=100.,
radars=self.radars)
# Benchmark definitions
self.benchmark = Benchmark(radars=self.radars,
radar_filter=self.radar_filter,
states=self.states)
detector = EuclidianDetector()
# ==========================================================================
# ========================= Filter(s) Generation ===========================
# ==========================================================================
## Specify the model and process noise you want!
## Comment the unused filters!
## One model filters
radar_filter = RadarFilterCV(q = 2300., radar = radar1, detector = detector,
x0 = x0, y0 = y0, z0 = z0)
radar_filter = MultipleRadarsFilterCV(q = 3040., radars = radars, detector = detector,
x0 = x0, y0 = y0, z0 = z0)
radar_filter = MultiplePeriodRadarsFilterCV(q = 3920., radars = pradars, detector = detector,
x0 = x0, y0 = y0, z0 = z0)
# ==========================================================================
# ========================= Attacker Generation ============================
# ==========================================================================
## Comment the unused attackers!
## Specify your own t0, time, radar_pos and radar!
## note radar1 => radar_pos = 0, radar2 => radar_pos = 1, etc.
## No attacker
attacker = None
## Attacker for 1 or 2 Radars
attacker = DOSAttacker(filter = radar_filter,
t0 = 200, time = 100, radar_pos = 1)
y=8000,
dt=dt_rad2,
r_std=5.,
theta_std=0.005,
phi_std=0.005)
pradars = [pradar1, pradar2]
# ================================ Detectors ===============================
# detector = None
# detector = MahalanobisDetector(error_rate = 0.05)
detector = EuclidianDetector(error_rate=0.05)
# ================================ Filters =================================
radar_filter_cv = MultiplePeriodRadarsFilterCV(q=200.,
detector=detector,
radars=pradars,
x0=x0,
y0=y0,
z0=z0)
radar_filter_ca = MultiplePeriodRadarsFilterCA(q=100.,
detector=detector,
radars=pradars,
x0=x0,
y0=y0,
z0=z0)
radar_filter_ct = MultiplePeriodRadarsFilterCT(q=350.,
detector=detector,
radars=pradars,
x0=x0,
y0=y0,
z0=z0)
filters = [radar_filter_cv, radar_filter_ca, radar_filter_ct]
class MultiplePeriodRadarsCVTestCase(unittest.TestCase):
def setUp(self):
self.radar1 = Radar(x=800,y=800)
self.radar2 = Radar(x=200,y=200)
radars = [self.radar1,self.radar2]
self.q = 10.
self.multiplef_cv = MultiplePeriodRadarsFilterCV(dim_x = 9, dim_z = 3, q = self.q,
radars = radars,
x0 = 100, y0 = 100)
# ==========================================================================
# ========================= Initialization tests ===========================
def test_initial_radar_positions(self):
positions = [[self.radar1.x,self.radar1.y,self.radar1.z],[self.radar2.x,self.radar2.y,self.radar2.z]]
computed_positions = self.multiplef_cv.radar_positions
self.assertEqual(computed_positions,positions)
def test_initial_R(self):
dt = self.multiplef_cv.dt
R = np.array([[1., 0. , 0. , 0., 0. , 0. ],
[0., 0.001, 0. , 0., 0. , 0. ],
[0., 0. , 0.001, 0., 0. , 0. ],
[0., 0. , 0. , 1., 0. , 0. ],
[0., 0. , 0. , 0., 0.001, 0. ],
[0., 0. , 0. , 0., 0. , 0.001]])
self.assertTrue(np.array_equal(self.multiplef_cv.R,R))
def test_initial_F(self):
dt = self.multiplef_cv.dt
F = np.array([[1,dt, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1,dt, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1,dt, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1]])
self.assertTrue(np.array_equal(self.multiplef_cv.F,F))
def test_initial_Q(self):
dt = self.multiplef_cv.dt
q = self.q
Q_block = np.array([[dt**3/2, dt**2/2, 0],
[dt**2/2, dt, 0],
[ 0, 0, 0]])
Q_block = q*Q_block
Q = block_diag(Q_block, Q_block, Q_block)
self.assertTrue(np.array_equal(self.multiplef_cv.Q,Q))
def test_tag_radars(self):
self.assertEqual(self.radar1.tag, 0)
self.assertEqual(self.radar2.tag, 1)
# ==========================================================================
# ========================= Q/F generation tests ===========================
def test_F_computing(self):
dt = 5.
F = np.array([[1,dt, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1,dt, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1,dt, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1]])
self.multiplef_cv.dt = dt
computed_F = self.multiplef_cv.compute_F(self.multiplef_cv.x)
self.assertTrue(np.array_equal(self.multiplef_cv.F,F))
self.assertTrue(np.array_equal(computed_F,F))
def test_Q_computing(self):
dt = 5.
q = 20.
Q_block = np.array([[dt**3/2, dt**2/2, 0],
[dt**2/2, dt, 0],
[ 0, 0, 0]])
Q_block = q*Q_block
Q = block_diag(Q_block, Q_block, Q_block)
self.multiplef_cv.dt = dt
computed_Q = self.multiplef_cv.compute_Q(q)
self.assertTrue(np.array_equal(self.multiplef_cv.Q,Q))
self.assertTrue(np.array_equal(computed_Q,Q))
# ==========================================================================
# ============================= HJacob/hx generation =======================
def test_HJacob_computing_tag_is_0(self):
X = np.array([[1000, 100, 10, 1000, 100, 10, 8000, 2, 10]]).T
tag = 0
x2 = X[0,0] - self.radar2.x
y2 = X[3,0] - self.radar2.y
z2 = X[6,0] - self.radar2.z
H = np.array([[x1/sqrt(x1**2 + y1**2 + z1**2), 0, 0, y1/sqrt(x1**2 + y1**2 + z1**2), 0, 0, z1/sqrt(x1**2 + y1**2 + z1**2),0 ,0],
[-y1/(x1**2 + y1**2), 0, 0, x1/(x1**2 + y1**2), 0, 0, 0, 0, 0],
[-x1*z1/(sqrt(x1**2 + y1**2)*(x1**2 + y1**2 + z1**2)), 0, 0, -y1*z1/(sqrt(x1**2 + y1**2)*(x1**2 + y1**2 + z1**2)), 0, 0, sqrt(x1**2 + y1**2)/(x1**2 + y1**2 + z1**2), 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0]])
computed_H = self.multiplef_cv.HJacob(X,tag = tag)
self.assertTrue(np.array_equal(computed_H,H))
def test_HJacob_computing_tag_is_0(self):
X = np.array([[1000, 100, 10, 1000, 100, 10, 8000, 2, 10]]).T
tag = 1
x2 = X[0,0] - self.radar2.x
y2 = X[3,0] - self.radar2.y
z2 = X[6,0] - self.radar2.z
H = np.array([[0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0],
[x2/sqrt(x2**2 + y2**2 + z2**2), 0, 0, y2/sqrt(x2**2 + y2**2 + z2**2), 0, 0, z2/sqrt(x2**2 + y2**2 + z2**2),0 ,0],
[-y2/(x2**2 + y2**2), 0, 0, x2/(x2**2 + y2**2), 0, 0, 0, 0, 0],
[-x2*z2/(sqrt(x2**2 + y2**2)*(x2**2 + y2**2 + z2**2)), 0, 0, -y2*z2/(sqrt(x2**2 + y2**2)*(x2**2 + y2**2 + z2**2)), 0, 0, sqrt(x2**2 + y2**2)/(x2**2 + y2**2 + z2**2), 0, 0]])
computed_H = self.multiplef_cv.HJacob(X,tag = tag)
self.assertTrue(np.array_equal(computed_H,H))
def test_hx_computing_tag_is_0(self):
X = np.array([[1000, 100, 10, 1000, 100, 10, 8000, 2, 10]]).T
tag = 0
x1 = X[0,0] - self.radar1.x
y1 = X[3,0] - self.radar1.y
z1 = X[6,0] - self.radar1.z
r1 = sqrt(x1**2 + y1**2 + z1**2)
theta1 = atan2(y1,x1)
phi1 = atan2(z1,sqrt(x1**2 + y1**2))
r2 = 0
theta2 = 0
phi2 = 0
Zk = np.array([[r1,theta1,phi1,r2,theta2,phi2]]).T
computed_Zk = self.multiplef_cv.hx(X, tag = tag)
self.assertTrue(np.array_equal(Zk,computed_Zk))
def test_hx_computing_tag_is_1(self):
X = np.array([[1000, 100, 10, 1000, 100, 10, 8000, 2, 10]]).T
tag = 1
x2 = X[0,0] - self.radar2.x
y2 = X[3,0] - self.radar2.y
z2 = X[6,0] - self.radar2.z
r1 = 0
theta1 = 0
phi1 = 0
r2 = sqrt(x2**2 + y2**2 + z2**2)
theta2 = atan2(y2,x2)
phi2 = atan2(z2,sqrt(x2**2 + y2**2))
Zk = np.array([[r1,theta1,phi1,r2,theta2,phi2]]).T
computed_Zk = self.multiplef_cv.hx(X, tag = tag)
self.assertTrue(np.array_equal(Zk,computed_Zk))
# ==========================================================================
# ========================= predict/update cycle tests =====================
def test_residual_of(self):
X = np.array([[1000, 100, 10, 1000, 100, 10, 8000, 2, 10]]).T
X_prior = np.array([[2000, 200, 20, 2000, 200, 20, 8000, 2, 10]]).T
z = np.array([[200, 10, 10]]).T
tag = 0
z_input = self.multiplef_cv.gen_complete_measurement(tag = tag, z = z)
computed_resid = z_input - self.multiplef_cv.HJacob(X,tag = 0)@X_prior
self.multiplef_cv.x = X
self.multiplef_cv.x_prior = X_prior
resid = self.multiplef_cv.residual_of(z = z, tag = tag)
self.assertTrue(np.array_equal(computed_resid,resid))
def test_predict(self):
X = np.array([[1000, 100, 10, 1000, 100, 10, 8000, 2, 10]]).T
filt = self.multiplef_cv
filt.x = X
predicted_X = [email protected]
predicted_P = [email protected]@filt.F.T + filt.Q
filt.predict()
self.assertTrue(np.array_equal(predicted_X,filt.x))
self.assertTrue(np.array_equal(predicted_P,filt.P))
self.assertTrue(np.array_equal(predicted_X,filt.x_prior))
self.assertTrue(np.array_equal(predicted_P,filt.P_prior))
def test_update_times(self):
X = np.array([[1000, 100, 10, 1000, 100, 10, 8000, 2, 10]]).T
tag = 1
time = 1.
z = np.array([[210, 9, 8]]).T
labeled_z = LabeledMeasurement(tag = tag, time = 1., value = z)
filt = self.multiplef_cv
filt.x = X
filt._last_t = 0.5
dt = time - filt._last_t
new_last_t = time
filt.predict()
filt.update(labeled_z)
self.assertEqual(new_last_t, filt._last_t)
self.assertEqual(dt, filt.dt)
def test_update(self):
X = np.array([[1000, 100, 10, 1000, 100, 10, 8000, 2, 10]]).T
tag = 0
z = np.array([[200, 10, 10]]).T
labeled_z = LabeledMeasurement(tag = tag, value = z, time = 1.)
filt = self.multiplef_cv
filt.x = X
filt.predict()
H = filt.HJacob(filt.x, tag = tag)
S = [email protected]@H.T + filt.R
K = [email protected]@inv(S)
hx = filt.hx(filt.x, tag = tag)
z_input = filt.gen_complete_measurement(tag = tag, z = z)
y = z_input - hx
new_X = filt.x + K@y
IKH = (filt._I - K@H)
new_P = ([email protected])@IKH.T + ([email protected])@K.T
filt.update(labeled_z)
self.assertTrue(np.allclose(filt.P,new_P))
self.assertTrue(np.allclose(filt.x,new_X))