|
|
|
@ -49,8 +49,7 @@ def get_select_fields_data(logs): |
|
|
|
|
return data |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def run_scenarios(scenario): |
|
|
|
|
logs = list(LogReader(get_url(TEST_ROUTE, TEST_SEG_NUM))) |
|
|
|
|
def run_scenarios(scenario, logs): |
|
|
|
|
if scenario == Scenario.BASE: |
|
|
|
|
pass |
|
|
|
|
|
|
|
|
@ -104,6 +103,11 @@ class TestLocationdScenarios(unittest.TestCase): |
|
|
|
|
- locationd kalman filter should never go unstable (we care mostly about yaw_rate, roll, gpsOK, inputsOK, sensorsOK) |
|
|
|
|
- faulty values should be ignored, with appropriate flags set |
|
|
|
|
""" |
|
|
|
|
|
|
|
|
|
@classmethod |
|
|
|
|
def setUpClass(cls): |
|
|
|
|
cls.logs = list(LogReader(get_url(TEST_ROUTE, TEST_SEG_NUM))) |
|
|
|
|
|
|
|
|
|
def test_base(self): |
|
|
|
|
""" |
|
|
|
|
Test: unchanged log |
|
|
|
@ -111,7 +115,7 @@ class TestLocationdScenarios(unittest.TestCase): |
|
|
|
|
- yaw_rate: unchanged |
|
|
|
|
- roll: unchanged |
|
|
|
|
""" |
|
|
|
|
orig_data, replayed_data = run_scenarios(Scenario.BASE) |
|
|
|
|
orig_data, replayed_data = run_scenarios(Scenario.BASE, self.logs) |
|
|
|
|
self.assertTrue(np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))) |
|
|
|
|
self.assertTrue(np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))) |
|
|
|
|
|
|
|
|
@ -123,7 +127,7 @@ class TestLocationdScenarios(unittest.TestCase): |
|
|
|
|
- roll: |
|
|
|
|
- gpsOK: False |
|
|
|
|
""" |
|
|
|
|
orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF) |
|
|
|
|
orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF, self.logs) |
|
|
|
|
self.assertTrue(np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))) |
|
|
|
|
self.assertTrue(np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))) |
|
|
|
|
self.assertTrue(np.all(replayed_data['gps_flag'] == 0.0)) |
|
|
|
@ -136,7 +140,7 @@ class TestLocationdScenarios(unittest.TestCase): |
|
|
|
|
- roll: |
|
|
|
|
- gpsOK: True for the first half, False for the second half |
|
|
|
|
""" |
|
|
|
|
orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF_MIDWAY) |
|
|
|
|
orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF_MIDWAY, self.logs) |
|
|
|
|
self.assertTrue(np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))) |
|
|
|
|
self.assertTrue(np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))) |
|
|
|
|
self.assertTrue(np.diff(replayed_data['gps_flag'])[512] == -1.0) |
|
|
|
@ -149,7 +153,7 @@ class TestLocationdScenarios(unittest.TestCase): |
|
|
|
|
- roll: |
|
|
|
|
- gpsOK: False for the first half, True for the second half |
|
|
|
|
""" |
|
|
|
|
orig_data, replayed_data = run_scenarios(Scenario.GPS_ON_MIDWAY) |
|
|
|
|
orig_data, replayed_data = run_scenarios(Scenario.GPS_ON_MIDWAY, self.logs) |
|
|
|
|
self.assertTrue(np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))) |
|
|
|
|
self.assertTrue(np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(1.5))) |
|
|
|
|
self.assertTrue(np.diff(replayed_data['gps_flag'])[505] == 1.0) |
|
|
|
@ -162,7 +166,7 @@ class TestLocationdScenarios(unittest.TestCase): |
|
|
|
|
- roll: |
|
|
|
|
- gpsOK: False for the middle section, True for the rest |
|
|
|
|
""" |
|
|
|
|
orig_data, replayed_data = run_scenarios(Scenario.GPS_TUNNEL) |
|
|
|
|
orig_data, replayed_data = run_scenarios(Scenario.GPS_TUNNEL, self.logs) |
|
|
|
|
self.assertTrue(np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))) |
|
|
|
|
self.assertTrue(np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))) |
|
|
|
|
self.assertTrue(np.diff(replayed_data['gps_flag'])[213] == -1.0) |
|
|
|
@ -176,7 +180,7 @@ class TestLocationdScenarios(unittest.TestCase): |
|
|
|
|
- roll: 0 |
|
|
|
|
- sensorsOK: False |
|
|
|
|
""" |
|
|
|
|
_, replayed_data = run_scenarios(Scenario.GYRO_OFF) |
|
|
|
|
_, replayed_data = run_scenarios(Scenario.GYRO_OFF, self.logs) |
|
|
|
|
self.assertTrue(np.allclose(replayed_data['yaw_rate'], 0.0)) |
|
|
|
|
self.assertTrue(np.allclose(replayed_data['roll'], 0.0)) |
|
|
|
|
self.assertTrue(np.all(replayed_data['sensors_flag'] == 0.0)) |
|
|
|
@ -189,7 +193,7 @@ class TestLocationdScenarios(unittest.TestCase): |
|
|
|
|
- roll: unchanged |
|
|
|
|
- inputsOK: False for some time after the spike, True for the rest |
|
|
|
|
""" |
|
|
|
|
orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY) |
|
|
|
|
orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs) |
|
|
|
|
self.assertTrue(np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))) |
|
|
|
|
self.assertTrue(np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))) |
|
|
|
|
self.assertTrue(np.diff(replayed_data['inputs_flag'])[500] == -1.0) |
|
|
|
@ -203,7 +207,7 @@ class TestLocationdScenarios(unittest.TestCase): |
|
|
|
|
- roll: 0 |
|
|
|
|
- sensorsOK: False |
|
|
|
|
""" |
|
|
|
|
_, replayed_data = run_scenarios(Scenario.ACCEL_OFF) |
|
|
|
|
_, replayed_data = run_scenarios(Scenario.ACCEL_OFF, self.logs) |
|
|
|
|
self.assertTrue(np.allclose(replayed_data['yaw_rate'], 0.0)) |
|
|
|
|
self.assertTrue(np.allclose(replayed_data['roll'], 0.0)) |
|
|
|
|
self.assertTrue(np.all(replayed_data['sensors_flag'] == 0.0)) |
|
|
|
@ -214,7 +218,7 @@ class TestLocationdScenarios(unittest.TestCase): |
|
|
|
|
Test: an accelerometer spike in the middle of the segment |
|
|
|
|
Expected Result: Right now, the kalman filter is not robust to small spikes like it is to gyroscope spikes. |
|
|
|
|
""" |
|
|
|
|
orig_data, replayed_data = run_scenarios(Scenario.ACCEL_SPIKE_MIDWAY) |
|
|
|
|
orig_data, replayed_data = run_scenarios(Scenario.ACCEL_SPIKE_MIDWAY, self.logs) |
|
|
|
|
self.assertTrue(np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2))) |
|
|
|
|
self.assertTrue(np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5))) |
|
|
|
|
|
|
|
|
|