{"metadata":{"kernelspec":{"language":"python","display_name":"Python 3","name":"python3"},"language_info":{"pygments_lexer":"ipython3","nbconvert_exporter":"python","version":"3.6.4","file_extension":".py","codemirror_mode":{"name":"ipython","version":3},"name":"python","mimetype":"text/x-python"}},"nbformat_minor":4,"nbformat":4,"cells":[{"cell_type":"markdown","source":"## Applying Kalman filter from pykalman package\n\nIn this notebook I check what can we get by applying a Kalman filter. The state space is 6 variables:\n1. x,y coordinates\n2. speed magnitude and direction\n3. speed of direction change\n4. acceleration for the speed magnitude\n\nIn this implementation I don't need rasterization, but I don't know how to turn it off, so I just set it to minimum size `raster_size = [1, 1]`. It saves some time. Additionally, to maximize CPU utilization I use parallelization into 4 threads by `joblib` package.\n\nThere are several hyper parameters, I selected them offline (not included in this notebook) by running `hyperopt` hyper parameter optimization package on 5000 samples from the training dataset.\n\nI think some of the lines below are not self-explanatory, if you have any questions please ask in the comments, I will clarify ASAP.","metadata":{}},{"cell_type":"code","source":"import os\nimport numpy as np\nfrom numpy import ma\nfrom torch.utils.data import DataLoader\nfrom tqdm import tqdm\nfrom pykalman import AdditiveUnscentedKalmanFilter\nfrom joblib import Parallel, delayed\nimport math","metadata":{"_uuid":"d629ff2d2480ee46fbb7e2d37f6b5fab8052498a","_cell_guid":"79c7e3d0-c299-4dcb-8224-4455121ee9b0","trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"!pip install --no-index -f ../input/kaggle-l5kit pip==20.2.2 >/dev/nul\n!pip install --no-index -f ../input/kaggle-l5kit -U l5kit > /dev/nul","metadata":{"collapsed":true,"jupyter":{"outputs_hidden":true},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"from l5kit.data import LocalDataManager, ChunkedDataset\nfrom l5kit.dataset import AgentDataset\nfrom l5kit.rasterization import build_rasterizer\nfrom l5kit.evaluation import write_pred_csv","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"cfg = {\n    'format_version': 4,\n    'model_params': {\n        'history_num_frames': 100,\n        'history_step_size': 1,\n        'history_delta_time': 0.1,\n        'future_num_frames': 50,\n        'future_step_size': 1,\n        'future_delta_time': 0.1\n    },\n    \n    'raster_params': {\n        'raster_size': [1, 1],\n        'pixel_size': [0.5, 0.5],\n        'ego_center': [0.25, 0.5],\n        'map_type': 'py_semantic',\n        'satellite_map_key': 'aerial_map/aerial_map.png',\n        'semantic_map_key': 'semantic_map/semantic_map.pb',\n        'dataset_meta_key': 'meta.json',\n        'filter_agents_threshold': 0.5\n    },\n    \n    'test_data_loader': {\n        'key': 'scenes/test.zarr',\n        'batch_size': 64,\n        'shuffle': False,\n        'num_workers': 4\n    }\n}","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"params = {\n    'Q_std': 0.00039548740307155435, \n    'acc_decay': 0.9466336363139376, \n    'acc_std': 0.006214926973039985, \n    'ang_lim': 0.0, \n    'ang_speed_std': 0.17307676721270504, \n    'ang_std': 0.03379979585323599, \n    'obs_std': 0.04800698362225296, \n    'speed_std': 1.6644181926567871}","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"DIR_INPUT = \"../input/lyft-motion-prediction-autonomous-vehicles\"\nos.environ[\"L5KIT_DATA_FOLDER\"] = DIR_INPUT\ndm = LocalDataManager(None)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"rasterizer = build_rasterizer(cfg, dm)\n\ntest_zarr = ChunkedDataset(dm.require(cfg['test_data_loader'][\"key\"])).open()\ntest_mask = np.load(f\"{DIR_INPUT}/scenes/mask.npz\")[\"arr_0\"]\ntest_dataset = AgentDataset(cfg, test_zarr, rasterizer, agents_mask=test_mask)\ntest_dataloader = DataLoader(test_dataset, \n                             shuffle=False, \n                             batch_size=cfg['test_data_loader'][\"batch_size\"], \n                             num_workers=cfg['test_data_loader'][\"num_workers\"])","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"def f(cs, ang_rng=None):\n    res = np.zeros(6)\n    res[0] = cs[0] + cs[2]*np.cos(cs[3])\n    res[1] = cs[1] + cs[2]*np.sin(cs[3])\n    res[2] = cs[2] + cs[5]\n    res[3] = cs[3] + cs[4]\n    if ang_rng is not None:\n        res[3] = np.clip(res[3], ang_rng[0], ang_rng[1])\n    res[4] = cs[4]\n    res[5] = params['acc_decay']*cs[5]\n    return res\n\ndef g(cs):\n    res = np.zeros(2)\n    res[0] = cs[0]\n    res[1] = cs[1]\n    return res","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"timestamps = []\nagent_ids = []\nfuture_coords_offsets_pd = []\n\nfor batch_idx, data in enumerate(tqdm(test_dataloader)):\n    \n    history_positions = data['history_positions'].cpu().numpy()\n    history_availabilities = data['history_availabilities'].cpu().numpy()\n    timestamp = data[\"timestamp\"].cpu().numpy()\n    track_id = data[\"track_id\"].cpu().numpy()\n    \n    def run(hp,ha,ts,ti):\n\n        measurements = hp[::-1]\n\n        ang_std = params['ang_std']\n        Q = params['Q_std']*np.diag([1, 1, params['speed_std'], ang_std**2, params['ang_speed_std']*ang_std**2, params['acc_std']])\n        m0 = measurements[-1]\n\n        kf = AdditiveUnscentedKalmanFilter(initial_state_mean = [m0[0],m0[1],0,0,0,0], \n                                           n_dim_obs=2,\n                                           transition_functions = f,\n                                           observation_functions = g,\n                                           transition_covariance = Q,\n                                           initial_state_covariance = Q,\n                                           observation_covariance = params['obs_std']**2*np.eye(2))\n\n        X = ma.array(measurements)\n        X[ha[::-1] < 0.5] = ma.masked\n\n        z = kf.smooth(X)\n\n        pred = np.zeros((51,6))\n        pred[0] = z[0][-1]\n        ang_rng = (z[0][-10:,3].min() - params['ang_lim'], z[0][-10:,3].max() + params['ang_lim'])\n        for i in range(1,51):\n            pred[i] = f(pred[i-1], ang_rng)\n        pred = pred[1:,:2]\n        \n        return ts, ti, np.expand_dims(pred,0)\n\n    res = Parallel(n_jobs=4)(delayed(run)(history_positions[i], history_availabilities[i], \n                                          timestamp[i], track_id[i]) for i in range(len(data['history_positions'])))\n    \n    timestamps.append(np.stack([r[0] for r in res]))\n    agent_ids.append(np.stack([r[1] for r in res]))\n    future_coords_offsets_pd.append(np.concatenate([r[2] for r in res]))\n\nprint(np.concatenate(future_coords_offsets_pd).shape)\nwrite_pred_csv(\"submission.csv\",\n       timestamps=np.concatenate(timestamps),\n       track_ids=np.concatenate(agent_ids),\n       coords=np.concatenate(future_coords_offsets_pd),\n      )","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"The end!","metadata":{}}]}