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_ca = MultiplePeriodRadarsFilterCA(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 = MultiplePeriodRadarsFilterCA(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)
# ================================ 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]
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]])
imm = RadarIMM(filters, mu, trans)
# =============================== Attackers ================================
class MultiplePeriodRadarsCATestCase(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_ca = MultiplePeriodRadarsFilterCA(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_ca.radar_positions
self.assertEqual(computed_positions, positions)
def test_initial_R(self):
dt = self.multiplef_ca.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_ca.R, R))
def test_initial_F(self):
dt = self.multiplef_ca.dt
dt2 = dt**2 / 2
F = np.array([[1, dt, dt2, 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, dt, dt2, 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, dt, dt2],
[0, 0, 0, 0, 0, 0, 0, 1, dt],
[0, 0, 0, 0, 0, 0, 0, 0, 1]])
self.assertTrue(np.array_equal(self.multiplef_ca.F, F))
def test_initial_Q(self):
dt = self.multiplef_ca.dt
q = self.q
Q = q * np.array(
[[0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, dt, 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, dt, 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, dt]])
self.assertTrue(np.array_equal(self.multiplef_ca.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.
dt2 = dt**2 / 2
F = np.array([[1, dt, dt2, 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, dt, dt2, 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, dt, dt2],
[0, 0, 0, 0, 0, 0, 0, 1, dt],
[0, 0, 0, 0, 0, 0, 0, 0, 1]])
self.multiplef_ca.dt = dt
computed_F = self.multiplef_ca.compute_F(self.multiplef_ca.x)
self.assertTrue(np.array_equal(self.multiplef_ca.F, F))
self.assertTrue(np.array_equal(computed_F, F))
def test_Q_computing(self):
dt = 5.
q = 20.
Q = q * np.array(
[[0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, dt, 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, dt, 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, dt]])
self.multiplef_ca.dt = dt
computed_Q = self.multiplef_ca.compute_Q(q)
self.assertTrue(np.array_equal(self.multiplef_ca.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_ca.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_ca.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_ca.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_ca.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_ca.gen_complete_measurement(tag=tag, z=z)
computed_resid = z_input - self.multiplef_ca.HJacob(X, tag=0) @ X_prior
self.multiplef_ca.x = X
self.multiplef_ca.x_prior = X_prior
resid = self.multiplef_ca.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_ca
filt.x = X
predicted_X = filt.F @ filt.x
predicted_P = filt.F @ filt.P @ 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_ca
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_ca
filt.x = X
filt.predict()
H = filt.HJacob(filt.x, tag=tag)
S = H @ filt.P @ H.T + filt.R
K = filt.P @ H.T @ 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 = (IKH @ filt.P) @ IKH.T + (K @ filt.R) @ K.T
filt.update(labeled_z)
self.assertTrue(np.allclose(filt.P, new_P))
self.assertTrue(np.allclose(filt.x, new_X))