def setUp(self):
# Radar & States generation
self.setUp_radar_states()
# Filter definition: CV model
self.radar_filter = RadarFilterCV(dim_x = 9, dim_z = 3, q = 100., radar = self.radar)
# Benchmark definition
self.benchmark = Benchmark(radars = self.radar, radar_filter = self.radar_filter, states = self.states)
def setUp(self):
# Radar & States generation
self.setUp_radar_states()
# Filter definition
## Classical models
self.radar_filter_ca = RadarFilterCA(dim_x = 9, dim_z = 3, q = 100., radar = self.radar)
self.radar_filter_cv = RadarFilterCV(dim_x = 9, dim_z = 3, q = 100., radar = self.radar)
## IMM with ca, cv and ct models
filters = [self.radar_filter_cv, self.radar_filter_ca]
mu = [0.5, 0.5]
trans = np.array([[0.999, 0.001],
[0.001, 0.999]])
self.radar_filter = RadarIMM(filters = filters, mu = mu, M = trans)
# Benchmark definition
self.benchmark = Benchmark(radars = self.radar, radar_filter = self.radar_filter, states = self.states)
def gen_env():
trajectory = Track()
states = trajectory.gen_landing()
x0 = states[0, 0]
y0 = states[0, 3]
z0 = states[0, 6]
radar = Radar(x=0, y=2000)
radar_filter_cv = RadarFilterCV(dim_x=9,
dim_z=3,
q=1.,
x0=x0,
y0=y0,
z0=z0,
radar=radar)
radar_filter_ca = RadarFilterCA(dim_x=9,
dim_z=3,
q=400.,
x0=x0,
y0=y0,
z0=z0,
radar=radar)
radar_filter_ct = RadarFilterCT(dim_x=9,
dim_z=3,
q=350.,
x0=x0,
y0=y0,
z0=z0,
radar=radar)
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 = IMMEstimator(filters, mu, trans)
benchmark_imm3 = Benchmark(radars=radar, radar_filter=imm, states=states)
benchmark_imm3.launch_benchmark(with_nees=True)
# ==========================================================================
# ========================= Detector Generation ============================
# ==========================================================================
## Comment the unused detectors!
detector = None
detector = MahalanobisDetector()
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
def setUp(self):
self.radar = Radar(x=0,y=0)
self.q = 10.
self.filter_cv = RadarFilterCV(dim_x = 9, dim_z = 3, q = self.q,radar = self.radar)
class RadarFilterCVTestCase(unittest.TestCase):
def setUp(self):
self.radar = Radar(x=0,y=0)
self.q = 10.
self.filter_cv = RadarFilterCV(dim_x = 9, dim_z = 3, q = self.q,radar = self.radar)
# ==========================================================================
# ========================= Initialization tests ===========================
def test_initial_F(self):
dt = self.filter_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.filter_cv.F,F))
def test_initial_R(self):
dt = self.filter_cv.dt
R = np.array([[1., 0. , 0. ],
[0., 0.001, 0. ],
[0., 0. , 0.001]])
self.assertTrue(np.array_equal(self.filter_cv.R,R))
def test_initial_Q(self):
dt = self.filter_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.filter_cv.Q,Q))
def test_initial_positions(self):
x0 = self.filter_cv.x[0,0]
y0 = self.filter_cv.x[3,0]
z0 = self.filter_cv.x[6,0]
self.assertEqual(x0, 1e-6)
self.assertEqual(y0, 1e-6)
self.assertEqual(z0, 1e-6)
def test_initial_velocities(self):
vx0 = self.filter_cv.x[1,0]
vy0 = self.filter_cv.x[4,0]
vz0 = self.filter_cv.x[7,0]
self.assertEqual(vx0, 1e-6)
self.assertEqual(vy0, 1e-6)
self.assertEqual(vz0, 1e-6)
def test_initial_accelerations(self):
vx0 = self.filter_cv.x[2,0]
vy0 = self.filter_cv.x[5,0]
vz0 = self.filter_cv.x[8,0]
self.assertEqual(vx0, 1e-6)
self.assertEqual(vy0, 1e-6)
self.assertEqual(vz0, 1e-6)
def test_initial_radar_positions(self):
x_rad = self.filter_cv.x_rad
y_rad = self.filter_cv.y_rad
z_rad = self.filter_cv.z_rad
self.assertEqual(x_rad, 0.)
self.assertEqual(y_rad, 0.)
self.assertEqual(z_rad, 0.)
# ==========================================================================
# ========================= 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.filter_cv.dt = dt
computed_F = self.filter_cv.compute_F(self.filter_cv.x)
self.assertTrue(np.array_equal(self.filter_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.filter_cv.dt = dt
computed_Q = self.filter_cv.compute_Q(q)
self.assertTrue(np.array_equal(self.filter_cv.Q,Q))
self.assertTrue(np.array_equal(computed_Q,Q))
# ==========================================================================
# ========================= hx/HJacob tests ================================
def test_HJacob_computing(self):
X = np.array([[1000, 100, 10, 1000, 100, 10, 8000, 2, 10]]).T
x = X[0,0]
y = X[3,0]
z = X[6,0]
H = np.array([[x/sqrt(x**2 + y**2 + z**2), 0, 0, y/sqrt(x**2 + y**2 + z**2), 0, 0, z/sqrt(x**2 + y**2 + z**2),0 ,0],
[-y/(x**2 + y**2), 0, 0, x/(x**2 + y**2), 0, 0, 0, 0, 0],
[-x*z/(sqrt(x**2 + y**2)*(x**2 + y**2 + z**2)), 0, 0, -y*z/(sqrt(x**2 + y**2)*(x**2 + y**2 + z**2)), 0, 0, sqrt(x**2 + y**2)/(x**2 + y**2 + z**2), 0, 0]])
computed_H = self.filter_cv.HJacob(X)
self.assertTrue(np.array_equal(computed_H,H))
def test_hx_computing(self):
X = np.array([[1000, 100, 10, 1000, 100, 10, 8000, 2, 10]]).T
x = X[0,0]
y = X[3,0]
z = X[6,0]
r = sqrt(x**2 + y**2 + z**2)
theta = atan2(y,x)
phi = atan2(z,sqrt(x**2 + y**2))
Zk = np.array([[r,theta,phi]]).T
computed_Zk = self.filter_cv.hx(X)
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
computed_resid = z - self.filter_cv.HJacob(X)@X_prior
self.filter_cv.x = X
self.filter_cv.x_prior = X_prior
resid = self.filter_cv.residual_of(z)
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.filter_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(self):
X = np.array([[1000, 100, 10, 1000, 100, 10, 8000, 2, 10]]).T
z = np.array([[200, 10, 10]]).T
filt = self.filter_cv
filt.x = X
filt.predict()
H = filt.HJacob(filt.x)
S = [email protected]@H.T + filt.R
K = [email protected]@inv(S)
hx = filt.hx(filt.x)
y = z - hx
new_X = filt.x + K@y
IKH = (filt._I - K@H)
new_P = ([email protected])@IKH.T + ([email protected])@K.T
filt.update(z)
self.assertTrue(np.array_equal(filt.P,new_P))
self.assertTrue(np.array_equal(filt.x,new_X))