scripts/Animations.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Copyright (C) 2024 co-pace GmbH (a subsidiary of Continental AG).\n",
    "# Licensed under the BSD-3-Clause License.\n",
    "# @author: Jonas Noah Michael Neuhöfer"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Animations\n",
    "\n",
    "Here we generate animations to visualise the problem.\n",
    "\n",
    "The rendered animations can be found in ``data/figures/``."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "- Loading package src\n",
      "-  Loading package src/filters\n",
      "-  Loading File 'src/utils.py'\n",
      "-   Loading File 'abstract.py'\n",
      "-   Loading File 'distributions.py'\n",
      "-   Loading File 'models.py'\n",
      "-   Loading File 'proposed.py'\n",
      "-   Loading File 'robust.py'\n",
      "-  Loading package src/visual\n",
      "-  Loading package src/eval\n",
      "-   Loading File 'simulate.py'\n",
      "-   Loading File 'Distr_test.py'\n",
      "-   Loading File 'Singer_test.py'\n",
      "-   Loading File 'showcase.py'\n",
      "-   Loading File 'animations.py'\n"
     ]
    }
   ],
   "source": [
    "%load_ext autoreload\n",
    "%autoreload 3\n",
    "%matplotlib widget\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib.animation as animation\n",
    "from matplotlib.lines import Line2D\n",
    "import matplotlib.patches as patches\n",
    "from matplotlib.patches import PathPatch\n",
    "from matplotlib.colors import LinearSegmentedColormap\n",
    "\n",
    "import scipy\n",
    "import scipy.stats as stats\n",
    "import sys\n",
    "    \n",
    "from context import filters, visual, eval, utils\n",
    "try:\n",
    "    from tqdm import tqdm\n",
    "    tqdm_pres = True\n",
    "except:\n",
    "    print(\"couldn't load tqdm\")\n",
    "    tqdm_pres = False\n",
    "#plt.rcParams['animation.ffmpeg_path'] = os.path.abspath(os.path.join(os.path.abspath(''), '../STFenv/lib/python3.8/site-packages/ffmpeg'))\n",
    "plt.rcParams.update(\n",
    "    {\n",
    "        \"text.usetex\": True,\n",
    "        \"text.latex.preamble\": r\"\\usepackage{amsmath}\\usepackage{amsfonts}\\usepackage{bm}\",\n",
    "        \"pdf.use14corefonts\": True,\n",
    "        \n",
    "        'font.family': 'serif',\n",
    "        'font.serif': ['Times'], #'Computer Modern Roman'\n",
    "        #'mathtext.fontset': 'custom',\n",
    "        #\"mathtext.rm\"  : \"Computer Modern Roman\",\n",
    "        #\"mathtext.it\"  : \"Computer Modern Roman:italic\",\n",
    "        #\"mathtext.bf\"  : \"Computer Modern Roman:bold\",\n",
    "    }\n",
    ")\n",
    "\n",
    "seed = eval.simulate.make_seed(0)\n",
    "skip = 4"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The autoreload extension is already loaded. To reload it, use:\n",
      "  %reload_ext autoreload\n"
     ]
    }
   ],
   "source": [
    "%load_ext autoreload\n",
    "%autoreload 3\n",
    "\n",
    "\n",
    "model, (x0, P0), (x,y,v,e,t) = eval.simulate.simulate_Singer(seed=seed, T=200, beta=0.25)\n",
    "x=x*np.array([1,0.75,1,1,1,1])[None,:,None]; y = y*np.array([1,0.75])[None,:,None]\n",
    "dt = t[1]-t[0]\n",
    "minx, miny, maxx, maxy = (x[:,0,0].min(), x[:,1,0].min(), x[:,0,0].max(), x[:,1,0].max())\n",
    "d = max(maxx-minx, maxy-miny)\n",
    "xlimmin, xlimmax, ylimmin, ylimmax = minx-0.05*d, maxx+0.05*d, miny-0.05*d, maxy+0.05*d\n",
    "midx, midy = (minx+maxx)/2, (miny+maxy)/2\n",
    "carlength = 0.05*d\n",
    "#plt.rcParams.update( {'font.serif': ['Computer Modern Roman']} )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "if False:\n",
    "    # This just tests that outliers out of frames are indicated correctly by arrows\n",
    "    N = 20\n",
    "    fig, ax = plt.subplots(1,1)\n",
    "    inner_rect = patches.Rectangle((minx, miny), maxx-minx, maxy-miny, edgecolor='r', facecolor='none')\n",
    "    lim_rect = patches.Rectangle((xlimmin, ylimmin), xlimmax-xlimmin, ylimmax-ylimmin, edgecolor='b', facecolor='none')\n",
    "    ax.add_patch(inner_rect)\n",
    "    ax.add_patch(lim_rect)\n",
    "    view = np.array([minx/4+3*maxx/4, 4*miny/5+maxy/5])\n",
    "\n",
    "    points = np.array([xlimmin-d/2, ylimmin-d/2])[None,:] + np.array([xlimmax-xlimmin+d, ylimmax-ylimmin+d])[None,:] * np.random.random(size=(N,2))\n",
    "    ax.scatter([view[0]],[view[1]], s=4, color=\"r\", marker=\"x\",linewidths=.5)\n",
    "\n",
    "    for i in range(N):\n",
    "        ax.scatter([points[i,0]],[points[i,1]], s=2, color=f\"C{i}\")\n",
    "        arrx,arry,arrdx,arrdy,out = visual.animations.arrow_for_outlier(view=view, obs=points[i],l=carlength, minx=minx, miny=miny, maxx=maxx, maxy=maxy)\n",
    "        if out:\n",
    "            arrow = ax.arrow(arrx,arry,arrdx,arrdy, width=carlength/4, head_length=carlength/2, head_width=carlength/2, fc=f\"C{i}\", ec=\"none\", alpha=0.4, zorder=4)\n",
    "\n",
    "    ax.set_xlim(xlimmin-d, xlimmax+d)\n",
    "    ax.set_ylim(ylimmin-d, ylimmax+d)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Animating only the agent:  99%|█████████▉| 199/201 [00:07<00:00, 26.07it/s]"
     ]
    }
   ],
   "source": [
    "upsampling_factor = 1\n",
    "skip_y = 4  # how many observations should be skipped. Leads to a less cluttered image. Or smoother state updates between observations\n",
    "away_pos = np.array([100,100,1,1,0,0])\n",
    "\n",
    "upsampledx, upsampledt = visual.animations.upsampling(x=x[1:], t=t, model=model, dt=(t[1]-t[0])/upsampling_factor)\n",
    "upsampledy, upsampledt_y = y[skip_y::skip_y,:,0], t[skip_y::skip_y]\n",
    "\n",
    "e_norms_F = np.sqrt(np.sum(e[skip_y::skip_y,:,0]**2, axis=1)) # these are F(2,model._R_distr().nu) distributed, here F(2,1)\n",
    "# we aim to make them chi-square chi2(2) distributed for the case of Gaussian noise instead of Students' t-distributed noise.\n",
    "e_norms_chi = stats.chi2.ppf(stats.f.cdf(e_norms_F/2, 2, 0.75), 2)\n",
    "upsampledy_Gaussian = x[1+skip_y::skip_y,:2,0]+(e_norms_chi/e_norms_F)[:,None]*e[skip_y::skip_y,:,0]\n",
    "\n",
    "plt.rcParams['savefig.bbox'] = 'tight'\n",
    "fig, ax  = plt.subplots(1, 1)\n",
    "ax.set_xlim(xlimmin, xlimmax)\n",
    "ax.set_ylim(ylimmin, ylimmax)\n",
    "ax.set_aspect(\"equal\")\n",
    "ax.set_xticks([],[])\n",
    "ax.set_yticks([],[])\n",
    "fig.set_tight_layout(True)\n",
    "\n",
    "#ax.plot(upsampledy[:,0], upsampledy[:,1], \"bo\", markersize=4)\n",
    "car = visual.animations.Car(state=upsampledx[0,:,0], ax=ax, model=model, length=carlength, past=0, predict=0, fc=\"#333333\", ec=\"none\")\n",
    "\n",
    "pbar = tqdm(desc=\"Animating only the agent\", total=len(upsampledt), leave=None, position=0, file=sys.stdout)\n",
    "def init1():\n",
    "    car.keep_past(0)\n",
    "    car.update(upsampledx[0,:,0])\n",
    "    car.keep_past(200*upsampling_factor)\n",
    "def animate_update1(frame):\n",
    "    pbar.update()\n",
    "    car.update(upsampledx[frame,:,0])\n",
    "\n",
    "if True:\n",
    "    ani = animation.FuncAnimation(fig, func=animate_update1, init_func=init1, frames=range(len(upsampledt)), repeat=True)\n",
    "    Writer = animation.writers['ffmpeg']\n",
    "    Writer = Writer(fps=30*upsampling_factor)\n",
    "    ani.save(f\"../data/figures/AnimationSimpleCar.mp4\", writer=Writer, dpi=200)\n",
    "plt.close(fig)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Animating only the agent: 100%|██████████| 201/201 [00:08<00:00, 24.08it/s]s]\n",
      "Animating the agent and observations: 100%|█████████▉| 200/201 [00:14<00:00, 15.38it/s]"
     ]
    }
   ],
   "source": [
    "fig, ax  = plt.subplots(1, 1)\n",
    "ax.set_xlim(xlimmin, xlimmax)\n",
    "ax.set_ylim(ylimmin, ylimmax)\n",
    "ax.set_aspect(\"equal\")\n",
    "ax.set_xticks([],[])\n",
    "ax.set_yticks([],[])\n",
    "fig.set_tight_layout(True)\n",
    "\n",
    "\n",
    "car = visual.animations.Car(state=upsampledx[1,:,0], ax=ax, model=model, length=carlength, past=0, predict=0, fc=\"#333333\", ec=\"none\", zorder=1)\n",
    "obserCar = visual.animations.Car(state=away_pos, ax=ax, model=model, length=carlength, past=0, predict=0, fc=\"blue\", ec=\"none\", alpha=0.4, zorder=2)\n",
    "obserCar2= visual.animations.Car(state=away_pos, ax=ax, model=model, length=carlength, past=0, predict=0, fc=[0,0,0.3], ec=\"none\", alpha=0.4, zorder=3)\n",
    "obserArrow = ax.arrow(*away_pos[:4], width=car.length/4, head_length=car.length/2, head_width=car.length/2, fc=\"blue\", ec=\"none\", alpha=0.4, zorder=4)\n",
    "\n",
    "pbar = tqdm(desc=\"Animating the agent and observations\", total=len(upsampledt), leave=None, position=0, file=sys.stdout)\n",
    "\n",
    "def init2():\n",
    "    init1()\n",
    "    global last_ty\n",
    "    last_ty = -1\n",
    "    obserCar.update(away_pos)\n",
    "    obserArrow.set_data(x=away_pos[0], y=away_pos[1])\n",
    "    obserCar.keep_past(0)\n",
    "    ax.legend([car.patch,obserCar.patch], ['ground truth position','noisy observations'], \n",
    "              handler_map={PathPatch: visual.animations.HandlerPathPatch()}, loc=\"upper right\").set_visible(True)\n",
    "\n",
    "def make_animate_update2(y_obs, times):\n",
    "    def animate_update2(frame):\n",
    "        car.update(upsampledx[frame,:,0])\n",
    "        global last_ty\n",
    "        if last_ty+1 < len(times) and upsampledt[frame]+1e-8 > times[last_ty+1]:\n",
    "            last_ty += 1\n",
    "            yx, yy = (y_obs[last_ty,0], y_obs[last_ty,1,])\n",
    "            if yx < xlimmin or yx > xlimmax or yy < ylimmin or yy > ylimmax:\n",
    "                x,y,dx,dy,out = visual.animations.arrow_for_outlier(view=upsampledx[frame,:2,0], obs=y_obs[last_ty],\n",
    "                                                                    l=car.length, minx=minx, miny=miny, maxx=maxx, maxy=maxy)\n",
    "                obserArrow.set_data(x=x,y=y,dx=dx,dy=dy)\n",
    "                #ax.plot([upsampledx[frame,0,0], x, yx,], [upsampledx[frame,1,0], y, yy])\n",
    "            else:\n",
    "                obserArrow.set_data(x=away_pos[0], y=away_pos[1])\n",
    "            obserCar.update([yx, yy, upsampledx[frame,2,0], upsampledx[frame,3,0], 0, 0])\n",
    "        pbar.update()\n",
    "    return animate_update2\n",
    "if True:\n",
    "    ani = animation.FuncAnimation(fig, func=make_animate_update2(upsampledy,upsampledt_y), init_func=init2, frames=range(len(upsampledt)), repeat=True)\n",
    "    Writer = animation.writers['ffmpeg']\n",
    "    Writer = Writer(fps=15*upsampling_factor)\n",
    "    ani.save(f\"../data/figures/AnimationStudentTNoise.mp4\", writer=Writer, dpi=200)\n",
    "plt.close(fig)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Animating the agent and observations: 100%|██████████| 201/201 [00:14<00:00, 13.65it/s]\n",
      "Animating our Method: 100%|█████████▉| 200/201 [00:16<00:00, 12.50it/s]"
     ]
    }
   ],
   "source": [
    "fig, ax  = plt.subplots(1, 1)\n",
    "ax.set_xlim(xlimmin, xlimmax)\n",
    "ax.set_ylim(ylimmin, ylimmax)\n",
    "ax.set_aspect(\"equal\")\n",
    "ax.set_xticks([],[])\n",
    "ax.set_yticks([],[])\n",
    "fig.set_tight_layout(True)\n",
    "\n",
    "mode = 2 # can be 0 (Kalman Filtering), 1 (Kalman Gating) 2 (Our Proposed)\n",
    "\n",
    "car = visual.animations.Car(state=upsampledx[0,:,0], ax=ax, model=model, length=carlength, past=0, predict=0, fc=\"#333333\", ec=\"none\", zorder=2)\n",
    "obserCar = visual.animations.Car(state=away_pos, ax=ax, model=model, length=carlength, past=0, predict=0, fc=\"blue\", ec=\"none\", alpha=0.4, zorder=3)\n",
    "filterCar= visual.animations.Car(state=away_pos, ax=ax, model=model, length=carlength, past=0, predict=0, fc=\"#C99F00\", ec=\"none\", zorder=1, alpha=0.5)\n",
    "obserArrow = ax.arrow(*away_pos[:4], width=car.length/4, head_length=car.length/2, head_width=car.length/2, fc=\"blue\", ec=\"none\", alpha=0.4, zorder=4)\n",
    "ellipse  = ax.plot(away_pos[:2], color=\"red\", alpha=0.2, zorder=1)[0]\n",
    "cancelX = ax.plot([],[], \"x\", mfc=[0.66,0,0], mec=[0.66,0,0], markersize=carlength*7.5, alpha=0.5, zorder=1.5)[0]\n",
    "\n",
    "pbar = tqdm(desc=(\"Animating the Kalman Filter\" if mode == 0 else (\"Animating the gated Kalman Filter\" if mode == 1 else \"Animating our Method\")), \n",
    "            total=len(upsampledt)*(2 if mode == 1 else 1), leave=None, position=0, file=sys.stdout)\n",
    "\n",
    "def init3():\n",
    "    init2()\n",
    "    obserCar.update(away_pos)\n",
    "    #car.keep_past(200*upsampling_factor)\n",
    "    car.patch.set(visible = False)\n",
    "    filterCar.update(x0)\n",
    "    filterCar.keep_past(0)\n",
    "    filterCar.keep_past(200*upsampling_factor)\n",
    "    filterCar.patch.set(fc=\"#C99F00\")\n",
    "    filterCar.cmap = LinearSegmentedColormap.from_list('custom_cmap', [\"#C99F00\", \"#C99F00\",[1,1,1,0],])\n",
    "    global filter_est\n",
    "    filter_est = None\n",
    "    #note.set_color([0,0,0,0])\n",
    "def init3_our():\n",
    "    init3()\n",
    "    filterCar.patch.set(fc=\"#ff4500\")\n",
    "    filterCar.cmap = LinearSegmentedColormap.from_list('custom_cmap', [\"#ff4500\", \"#ff4500\",[1,1,1,0],])\n",
    "    #filterCar.keep_past(filterCar.past)\n",
    "    #note.set_text(\"our filter\")\n",
    "    #note.set_color(\"black\")\n",
    "\n",
    "def make_init3(inmode):\n",
    "    if inmode == -1:\n",
    "        def init3_new():\n",
    "            init3()\n",
    "            ax.legend([Line2D([],[],color=\"black\"), obserCar.patch,filterCar.patch], \n",
    "                    ['ground truth position','Gaussian observations','Kalman Filter'], \n",
    "                    handler_map={PathPatch: visual.animations.HandlerPathPatch()}, loc=\"upper right\").set_visible(True)\n",
    "    elif inmode == 0:\n",
    "        def init3_new():\n",
    "            init3()\n",
    "            ax.legend([Line2D([],[],color=\"black\"), obserCar.patch,filterCar.patch], \n",
    "                    ['ground truth position','noisy observations','Kalman Filter'], \n",
    "                    handler_map={PathPatch: visual.animations.HandlerPathPatch()}, loc=\"upper right\").set_visible(True)\n",
    "    elif inmode == 1:\n",
    "        def init3_new():\n",
    "            init3()\n",
    "            ax.legend([Line2D([],[],color=\"black\"), obserCar.patch, filterCar.patch, Line2D([],[],color=\"red\",alpha=0.2)], \n",
    "                    ['ground truth position','noisy observations','gated Kalman Filter', 'discarding boundary'], \n",
    "                    handler_map={PathPatch: visual.animations.HandlerPathPatch()}, loc=\"upper right\").set_visible(True)\n",
    "    else:\n",
    "        def init3_new():\n",
    "            init3_our()\n",
    "            filterCar.patch.set(fc=\"#ff4500\")\n",
    "            ax.legend([Line2D([],[],color=\"black\"), obserCar.patch, filterCar.patch,], \n",
    "                    ['ground truth position','noisy observations','proposed method'], \n",
    "                    handler_map={PathPatch: visual.animations.HandlerPathPatch()}, loc=\"upper right\").set_visible(True)\n",
    "    return init3_new\n",
    "\n",
    "def make_animate_update3(y_obs, times, filter, draw_ellipse_p=None, elongate=1, **filterkwargs): # upsampledy, upsampledt_y\n",
    "    prev_update2 = make_animate_update2(y_obs, times)\n",
    "    F_matrices = {}\n",
    "    def F(dt):\n",
    "        if dt not in F_matrices:\n",
    "            F_matrices[dt] = model._F(dt)\n",
    "        return F_matrices[dt]\n",
    "    Q_matrices = {}\n",
    "    def Q(dt):\n",
    "        if dt not in Q_matrices:\n",
    "            Q_matrices[dt] = model._Q(dt)\n",
    "        return Q_matrices[dt]\n",
    "    def animate_update3(frame):\n",
    "        frame = int(frame/elongate)\n",
    "        prev_update2(frame)\n",
    "        global filter_est\n",
    "        global filtered_t\n",
    "        if filter_est == None:\n",
    "            filter_est, filtered_t = filter(model=model, mean=x0, covar=P0, **filterkwargs).filter(y_obs,times).get_all_state_distr()\n",
    "            filterCar.update(filter_est.mean()[0,:,0])\n",
    "            filterCar.keep_past(int(200*upsampling_factor*elongate))\n",
    "            car.keep_past(int(200*upsampling_factor*elongate))\n",
    "            #if elongate-1 > 1e-8:\n",
    "            #   note.set_text(f\"{elongate:.2f}x slower\")\n",
    "            #   note.set_color(\"black\")\n",
    "            #elif elongate-1 < -1e-8:\n",
    "            #   note.set_text(f\"{1/elongate:.2f}x faster\")\n",
    "            #   note.set_color(\"black\")\n",
    "        mean_last, covar_last = filter_est.mean()[last_ty,:,0], filter_est.cov()[last_ty,:,:]\n",
    "        filter_dt = upsampledt[frame] - times[last_ty] if last_ty > 0 and upsampledt[frame]+1e-8 > times[last_ty] else 0\n",
    "        mean_pred = F(filter_dt) @ filter_est.mean()[last_ty+1,:,0]\n",
    "        if draw_ellipse_p is not None and (frame > 0 and last_ty >= 0 and upsampledt[frame-1]+1e-8 < times[last_ty]):\n",
    "            pred_dt = filtered_t[last_ty+1]-filtered_t[last_ty]\n",
    "            covar_pred = (F(pred_dt)@covar_last@F(pred_dt).T +Q(pred_dt))[:2,:2] + model._R #causes epilepsy\n",
    "            ellipse_data = utils.confidence_ellipse(covar_pred,p=draw_ellipse_p)(np.linspace(0,1,1001))\n",
    "            # need to do some weird shit because the sensor points are at the front-wheels (which might be outside the ellipse when the midpoint is not)\n",
    "            car_ori = np.abs(obserCar.rear_pos[:2]-obserCar.front_pos[:2])/obserCar.length\n",
    "            ellipse_scaling = (np.max(ellipse_data,axis=1)-car_ori)/np.max(ellipse_data,axis=1)\n",
    "            ellipse_scaling = np.array([max(ellipse_scaling[0],0.5),max(ellipse_scaling[1],0.5)])[:,None]\n",
    "            ellipse_data *= ellipse_scaling\n",
    "            ellipse.set_data(ellipse_data+mean_pred[:2,None] )\n",
    "            if 'gating' in filterkwargs:\n",
    "                res = y_obs[last_ty] - (F(pred_dt)@mean_last)[:2]\n",
    "                gamma = res.T @ scipy.linalg.solve(covar_pred, res, check_finite=False, assume_a='pos')\n",
    "                if gamma > scipy.stats.chi2(2).ppf(filterkwargs['gating']):\n",
    "                    obserposx, obserposy = (obserCar.rear_pos[:2]+obserCar.front_pos[:2])/2\n",
    "                    if obserposx < xlimmin or obserposx > xlimmax or obserposy < ylimmin or obserposy > ylimmax:\n",
    "                        obserposx, obserposy, dx, dy, out = visual.animations.arrow_for_outlier(\n",
    "                                obs=obserCar.front_pos[:2], view=upsampledx[frame,:2,0],\n",
    "                                l=car.length, minx=minx, miny=miny, maxx=maxx, maxy=maxy  )\n",
    "                        obserposx += dx; obserposy += dy\n",
    "                    #ax.plot([upsampledx[frame,0,0],obserposx],[upsampledx[frame,1,0],obserposy], \"--\")\n",
    "                    cancelX.set_data([obserposx], [obserposy])\n",
    "                else:\n",
    "                    cancelX.set_data([],[])\n",
    "        filterCar.update(mean_pred)\n",
    "\n",
    "    return animate_update3\n",
    "if True:\n",
    "    if mode == 0:\n",
    "        ani = animation.FuncAnimation(fig, func=make_animate_update3(upsampledy,upsampledt_y,filters.basic.KalmanFilter), init_func=make_init3(mode), frames=range(len(upsampledt)), repeat=True)\n",
    "    elif mode == 1:\n",
    "        ani = animation.FuncAnimation(fig, func=make_animate_update3(upsampledy,upsampledt_y,filters.basic.KalmanFilter,draw_ellipse_p=0.99,elongate=2,gating=0.99), init_func=make_init3(mode), frames=range(len(upsampledt)*2), repeat=True)\n",
    "    elif mode == 2:\n",
    "        ani = animation.FuncAnimation(fig, func=make_animate_update3(upsampledy,upsampledt_y,filters.proposed.StudentTFilter, nu=3), init_func=make_init3(mode), frames=range(len(upsampledt)), repeat=True)\n",
    "    Writer = animation.writers['ffmpeg']\n",
    "    Writer = Writer(fps=20*upsampling_factor)\n",
    "    ani.save(f\"../data/figures/AnimationMethod{mode}.mp4\", writer=Writer, dpi=200)\n",
    "plt.close(fig)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "filter_class_kwargs_GT=[\n",
    "    (filters.proposed.StudentTFilter_GT, {\"nu\":3}, lambda x,y,v,e,t: {\"GTx\":x, \"GTe\":e}),\n",
    "    (filters.proposed.StudentTFilter, {\"nu\":3}, lambda x,y,v,e,t: {}),\n",
    "    (filters.robust.Huang_SSM, {\"separate\":True, \"SSM\":\"log\", \"nu_SSM\":3, \"process_non_gaussian\": True, \"sigma_SSM\": 1, \"gating\":1}, lambda x,y,v,e,t: {}),\n",
    "    (filters.robust.Agamennoni_VBF, {\"nu\":8, \"gating\":1}, lambda x,y,v,e,t: {}),\n",
    "    (filters.robust.chang_RKF, {\"alpha\":0.01}, lambda x,y,v,e,t: {}),\n",
    "    (filters.robust.chang_ARKF, {\"alpha\":0.01}, lambda x,y,v,e,t: {}),\n",
    "    (filters.robust.Saerkkae_VBF, {\"rho\":0, \"gating\":1}, lambda x,y,v,e,t: {}),\n",
    "    (filters.robust.roth_STF, {\"state_nu\":5, \"process_gamma\":11, \"obs_delta\":2.5}, lambda x,y,v,e,t: {}),\n",
    "    (filters.basic.KalmanFilter, {}, lambda x,y,v,e,t: {\"gating\":0.99}),\n",
    "]\n",
    "colors = ['#800000', '#ff4500', '#000075', '#006400', '#4363d8', '#42d4f4', '#00ff00', '#f58231', '#9a9a9a',]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Animating our Method: 100%|██████████| 201/201 [00:17<00:00, 11.62it/s]/s]\n",
      "Failed to remove car collection\n",
      "name 'filter_cars' is not defined\n",
      "Animating Multiple robust filters: 100%|██████████| 201/201 [00:16<00:00, 13.21it/s]"
     ]
    }
   ],
   "source": [
    "def init_multiple_filters():\n",
    "    init3()\n",
    "    ax.legend([Line2D([],[],color=\"black\")],[\"nothing\"]).set_visible(False)\n",
    "    filterCar.update(away_pos)\n",
    "    filterCar.keep_past(0)\n",
    "    car.patch.set_visible(True)\n",
    "    car.patch.set(alpha=0.75,fc=\"black\")\n",
    "    global filter_cars, filter_ests\n",
    "    try:\n",
    "        for i in range(len(filter_cars)):\n",
    "            filter_cars[i].patch.set(visible = False)\n",
    "            filter_cars[i]._past.set(visible = False)\n",
    "    except Exception as e:\n",
    "        print(\"Failed to remove car collection\")\n",
    "        print(e)\n",
    "        pass\n",
    "\n",
    "    filter_cars = None; filter_ests = None\n",
    "\n",
    "multiple_filters_seed = seed\n",
    "def make_animate_multiple_filters(elongate=1):\n",
    "\n",
    "    F_matrices = {}\n",
    "    def F(dt):\n",
    "        if dt not in F_matrices:\n",
    "            F_matrices[dt] = model._F(dt)\n",
    "        return F_matrices[dt]\n",
    "\n",
    "    def animate_multiple_filters(frame):\n",
    "        frame = int(frame/elongate)\n",
    "        global filter_cars, filter_ests\n",
    "        global _upsampledx, _upsampledt, _upsampledy, _upsampledt_y, _GTx, _GTy, _GTe, _GTv, _GTt\n",
    "        global last_ty, multiple_filters_seed\n",
    "        is_defined = False\n",
    "        try:\n",
    "            _upsampledt_y\n",
    "            is_defined = True\n",
    "        except Exception:\n",
    "            pass\n",
    "        if is_defined and last_ty+1 < len(_upsampledt_y) and _upsampledt[frame]+1e-8 > _upsampledt_y[last_ty+1]:\n",
    "            last_ty += 1\n",
    "            yx, yy = (_upsampledy[last_ty,0], _upsampledy[last_ty,1,])\n",
    "            if yx < xlimmin or yx > xlimmax or yy < ylimmin or yy > ylimmax:\n",
    "                x,y,dx,dy,out = visual.animations.arrow_for_outlier(view=_upsampledx[frame,:2,0], obs=_upsampledy[last_ty,:2],\n",
    "                                            l=car.length, minx=minx, miny=miny, maxx=maxx, maxy=maxy)\n",
    "                obserArrow.set_data(x=x,y=y,dx=dx,dy=dy)\n",
    "                #ax.plot([upsampledx[frame,0,0], x, yx,], [upsampledx[frame,1,0], y, yy])\n",
    "            else:\n",
    "                obserArrow.set_data(x=away_pos[0], y=away_pos[1])\n",
    "            obserCar.update([yx, yy, _upsampledx[frame,2,0], _upsampledx[frame,3,0], 0, 0])\n",
    "\n",
    "        if filter_ests == None:\n",
    "            # At the first frame\n",
    "            _model, (_x0, _P0), (_x,_y,_v,_e,_t) = eval.simulate.simulate_Singer(seed=multiple_filters_seed, T=200, beta=0.25)\n",
    "            multiple_filters_seed = eval.simulate.make_seed(multiple_filters_seed)\n",
    "            multiple_filters_seed = eval.simulate.make_seed(multiple_filters_seed)\n",
    "            _x=_x*np.array([1,0.75,1,1,1,1])[None,:,None]; _y = _y*np.array([1,0.75])[None,:,None]\n",
    "            _upsampledx, _upsampledt = visual.animations.upsampling(x=_x[1:], t=_t, model=_model, dt=(_t[1]-_t[0])/upsampling_factor)\n",
    "            _upsampledy, _upsampledt_y = _y[skip_y::skip_y,:,:], _t[skip_y::skip_y]\n",
    "            _upsampledy = _upsampledy[:,:,0]\n",
    "\n",
    "            _minx, _miny, _maxx, _maxy = (_x[:,0,0].min(), _x[:,1,0].min(), _x[:,0,0].max(), _x[:,1,0].max())\n",
    "            x_trans = lambda xpos: (xpos-_minx)/(_maxx-_minx)*(maxx-minx)+minx\n",
    "            y_trans = lambda ypos: (ypos-_miny)/(_maxy-_miny)*(maxy-miny)+miny\n",
    "            def trans(pos):\n",
    "                pos[:,0] = (pos[:,0]-_minx)/(_maxx-_minx)*(maxx-minx)+minx\n",
    "                pos[:,1] = (pos[:,1]-_miny)/(_maxy-_miny)*(maxy-miny)+miny\n",
    "            #_GTx, _GTy, _GTe, _GTv, _GTt = _x[skip_y::skip_y,:,:],_upsampledy,_v[skip_y::skip_y,:,:],_e[skip_y::skip_y,:,:],_upsampledt_y\n",
    "            #_x0[0] = x_trans(_x0[0]); _x0[1] = x_trans(_x0[1]); \n",
    "            #for _GT in [_GTx, _GTy, _GTe, _GTv, _upsampledx]:\n",
    "            #    _GT[:,0,:] = x_trans(_GT[:,0,:]); _GT[:,1,:] = y_trans(_GT[:,1,:])\n",
    "            \n",
    "            filter_ests = []; filter_cars = []\n",
    "            for i in range(len(filter_class_kwargs_GT)):\n",
    "                filter_est, filtered_t = filter_class_kwargs_GT[i][0](\n",
    "                        model=_model, mean=_x0, covar=_P0, **filter_class_kwargs_GT[i][1], \n",
    "                        **(filter_class_kwargs_GT[i][2](_x[skip_y::skip_y,:,:],_y[skip_y::skip_y,:,:],_v[skip_y::skip_y,:,:],_e[skip_y::skip_y,:,:],_t[skip_y::skip_y]))\n",
    "                    ).filter(_upsampledy,_upsampledt_y).get_all_state_distr()\n",
    "                filter_ests.append(filter_est.mean())\n",
    "                trans(filter_ests[i])\n",
    "                filter_cars.append(visual.animations.Car(\n",
    "                        state=_x0, ax=ax, model=model, length=carlength, past=0, \n",
    "                        predict=0, fc=colors[i], ec=\"none\"\n",
    "                    ))\n",
    "            trans(_upsampledx)\n",
    "            trans(_upsampledy)\n",
    "            for i in range(len(filter_class_kwargs_GT)):\n",
    "                filter_cars[i].update(filter_ests[i][0,:,0])\n",
    "                filter_cars[i].keep_past(0)\n",
    "                filter_cars[i].keep_past(int(40*upsampling_factor*elongate))\n",
    "                filter_cars[i].patch.set(visible = False)\n",
    "                car.update(_upsampledx[frame,:,0])\n",
    "                car.keep_past(0)\n",
    "                car.keep_past(int(40*upsampling_factor*elongate))\n",
    "            if elongate-1 > 1e-8:\n",
    "                note.set_text(f\"{elongate:.2f}x slower\")\n",
    "                note.set_color(\"black\")\n",
    "            elif elongate-1 < -1e-8:\n",
    "                note.set_text(f\"{1/elongate:.2f}x faster\")\n",
    "                note.set_color(\"black\")\n",
    "            \n",
    "        car.update(_upsampledx[frame,:,0])\n",
    "        filter_dt = upsampledt[frame] - _upsampledt_y[last_ty] if last_ty > 0 and upsampledt[frame]+1e-8 > _upsampledt_y[last_ty] else 0\n",
    "        for i in range(len(filter_class_kwargs_GT)):\n",
    "            filter_cars[i].update(F(filter_dt) @ filter_ests[i][last_ty+1,:,0])\n",
    "            \n",
    "        pbar.update()\n",
    "\n",
    "    return animate_multiple_filters\n",
    "\n",
    "\n",
    "fig, ax  = plt.subplots(1, 1, figsize=(10,6))\n",
    "ax.set_xlim(xlimmin, xlimmax)\n",
    "ax.set_ylim(ylimmin, ylimmax)\n",
    "ax.set_aspect(\"equal\")\n",
    "#ax.set_xticks([],[])\n",
    "#ax.set_yticks([],[])\n",
    "\n",
    "car = visual.animations.Car(state=away_pos, ax=ax, model=model, length=carlength, past=0, predict=0, fc=\"#333333\", ec=\"none\")\n",
    "obserCar = visual.animations.Car(state=away_pos, ax=ax, model=model, length=carlength, past=0, predict=0, fc=\"blue\", ec=\"none\", alpha=0.4)\n",
    "filterCar= visual.animations.Car(state=away_pos, ax=ax, model=model, length=carlength, past=0, predict=0, fc=\"#9a9a9a\", ec=\"none\")#, alpha=0.4)\n",
    "obserArrow = ax.arrow(*away_pos[:4], width=car.length/4, head_length=car.length/2, head_width=car.length/2, fc=\"blue\", ec=\"none\", alpha=0.4)\n",
    "ellipse = ax.plot(away_pos[:2], color=\"red\", alpha=0.2)[0]\n",
    "cancelX = ax.plot([],[], \"x\", mfc=[0.66,0,0], mec=[0.66,0,0], markersize=carlength*7.5, alpha=0.5)[0]\n",
    "\n",
    "comment = ax.text(x=midx, y=midy, ha=\"center\", va=\"center\", fontsize=22, s=\"\")\n",
    "note = ax.text(x=maxx, y=maxy, ha=\"right\", va=\"top\", fontsize=14, s=f\"\")\n",
    "\n",
    "\n",
    "if True:\n",
    "    pbar = tqdm(desc=\"Animating Multiple robust filters\", total=len(upsampledt), leave=None, position=0, file=sys.stdout)\n",
    "\n",
    "    ani = animation.FuncAnimation(fig, func=make_animate_multiple_filters(), init_func=init_multiple_filters, frames=range(len(upsampledt)), repeat=True)\n",
    "    Writer = animation.writers['ffmpeg']\n",
    "    Writer = Writer(fps=30*upsampling_factor)\n",
    "    ani.save(f\"../data/figures/AnimationMultipleFilters.mp4\", writer=Writer, dpi=200)\n",
    "plt.close(fig)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/tmp/ipykernel_80853/2445542904.py:125: UserWarning: set_ticklabels() should only be used with a fixed number of ticks, i.e. after set_ticks() or using a FixedLocator.\n",
      "  ax_error.set_xticklabels(['our, oracle\\n$a(e)$, $b(x)$', 'our', 'Huang 2020', 'Agamennoni 2011', 'Chang 2014', 'Chang 2014', 'Särkkä 2012', 'Roth 2013', 'Kalman Filter'], rotation=-90)\n"
     ]
    }
   ],
   "source": [
    "blend_in__frames = 30*upsampling_factor\n",
    "blend_out_frames = 30*upsampling_factor\n",
    "stay_frames = 90*upsampling_factor\n",
    "show_comment_frames = blend_in__frames + blend_out_frames + stay_frames + 2\n",
    "\n",
    "def init_comment():\n",
    "    ax.set_visible(True)\n",
    "    car.update(away_pos)\n",
    "    car.keep_past(0)\n",
    "    obserCar.update(away_pos)\n",
    "    obserCar.keep_past(0)\n",
    "    filterCar.update(away_pos)\n",
    "    filterCar.keep_past(0)\n",
    "    obserArrow.set_data(x=away_pos[0], y=away_pos[1], dx=away_pos[2], dy=away_pos[3])\n",
    "    ellipse.set_data([],[])\n",
    "    comment.set_fontsize(22)\n",
    "    comment.set_color([0,0,0,0])\n",
    "    note.set_color([0,0,0,0])\n",
    "    cancelX.set_data([], [])\n",
    "    ax_time.set_visible(False); ax_error.set_visible(False)\n",
    "    ax.legend([Line2D([],[],color=\"black\")],[\"nothing\"]).set_visible(False)\n",
    "\n",
    "def init_qualitative():\n",
    "    init_comment()\n",
    "    ax.set_visible(False)\n",
    "    ax_time.set_visible(True); ax_error.set_visible(True)\n",
    "\n",
    "def empty(frame):\n",
    "    pbar.update()\n",
    "\n",
    "def init_Title():\n",
    "    init_comment()\n",
    "    comment.set_text(\"$\\\\textbf{An Outlier-Robust and Efficient}$\\n$\\\\textbf{Bayesian Filter}$\")\n",
    "def init_FreeAgent():\n",
    "    init_comment()\n",
    "    comment.set_text(\"Imagine a highly maneuverable,\\nfreely moving agent\")\n",
    "def init_Tracking_Gaus():\n",
    "    init_comment()\n",
    "    comment.set_text(\"When \\\\textbf{tracking} the agent from afar,\\nonly \\\\textbf{noisy measurements} of it\\nare available\") # spacing?\n",
    "def init_KF_intro05():\n",
    "    init_comment()\n",
    "    comment.set_text(\"We want to \\\\textbf{reconstruct} the trajectory\\nonly from these noisy measurements\")\n",
    "def init_KF_intro():\n",
    "    init_comment()\n",
    "    comment.set_text(\"If this noise is \\\\textbf{Gaussian}, then the\\n\\\\textbf{Kalman Filter} finds \\\\textbf{optimal}\\nestimates \\\\textbf{efficiently} \")\n",
    "def init_KF_outliers():\n",
    "    init_comment()\n",
    "    comment.set_text(\"However, \\\\textbf{real-world} measurements are\\noften \\\\textbf{non-Gaussian} and contain,\\nfor example, \\\\textbf{outliers}\")\n",
    "def init_Gating1():\n",
    "    init_comment()\n",
    "    comment.set_text(\"To \\\\textbf{counteract} outliers, a common\\npractice is to try to \\\\textbf{classify} and\\nsubsequently \\\\textbf{ignore} them\")\n",
    "def init_Gating2():\n",
    "    init_comment()\n",
    "    comment.set_text(\"This '\\\\textbf{Gating}' declares measurements\\noutside a \\\\textbf{confidence ellipse} around\\nthe \\\\textbf{hypothesis} as outliers\")\n",
    "def init_Gating3():\n",
    "    init_comment()\n",
    "    comment.set_text(\"However, this may \\\\textbf{discard}\\n\\\\textbf{valid measurements}\")\n",
    "def init_Proposed1():\n",
    "    init_comment()\n",
    "    #comment.set_text(\"\\\\textbf{Prior work} has modelled such non-Gaussian\\nmeasurements with \\\\textbf{more general} multivariate\\n\\\\textbf{Student-t} distributions\") #better spacing?\n",
    "    comment.set_text(\"In \\\\textbf{prior work}, such non-Gaussian\\nmeasurements are modeled with\\nmore general multivariate\\n\\\\textbf{Student-t} distributions\") #three lines?\n",
    "def init_Proposed2():\n",
    "    init_comment()\n",
    "    comment.set_text(\"Our proposed filter and smoother\\nare leveraging the following equality\\nof Student-t densities $t_{\\\\omega }(x|\\\\mu, \\\\Sigma)$\")\n",
    "    #comment.set_text(\"We leverage \\\\textbf{properties} of the Student-t\\ndistribution directly to propose a \\\\textbf{robust} and\\n\\\\textbf{efficient} \\\\textbf{filter} and \\\\textbf{smoother}\")\n",
    "def init_Proposed3():\n",
    "    init_comment()\n",
    "    comment.set_text(\"$t_{\\\\omega }(e|0, R) \\\\cdot t_{\\\\omega +m} \\\\left( x | \\\\mu, P\\\\right) \\\\cdot  t_{\\\\omega +m+n} \\\\left( v | 0, Q\\\\right)$\"\n",
    "                     +\"\\n\"+\"$= t_{\\\\omega }   \\\\left(   \\\\left[    \\\\begin{array}{c} x\\\\\\\\v\\\\\\\\e \\\\end{array}    \\\\right]\\\\right|  \\\\left[    \\\\begin{array}{c} \\\\mu\\\\\\\\0\\\\\\\\0 \\\\end{array}    \\\\right] ,\\\\left. \\\\left[    \\\\begin{array}{ccc} a(e) P&           0  &  0\\\\\\\\0&        a(e) b(x) Q &  0 \\\\\\\\ 0&           0  &  R \\\\end{array}    \\\\right]  \\\\right)$\"\n",
    "                     +\"\\n\\nwith\\n\"+\"$a(e) = \\\\tfrac{\\\\omega +m}{\\\\omega +e^\\\\top R^{-1}e}$ and $b(x) = \\\\tfrac{\\\\omega +m+n}{\\\\omega +m+ (x-\\\\mu)^{ \\\\top}  P^{-1}(x-\\\\mu)}$\")\n",
    "    comment.set_fontsize(18)\n",
    "def init_Proposed4():\n",
    "    init_comment()\n",
    "    comment.set_text(\"This allows for \\\\textbf{localised} approximations\\nof joint Student-t distributions\")\n",
    "def init_Proposed5():\n",
    "    init_comment()\n",
    "    comment.set_text(\"Our methods then arise from other\\nbasic mathematical properties of\\nStudent-t distributions\")\n",
    "def init_Multiple():\n",
    "    init_comment()\n",
    "    comment.set_text(\"We further compare our methods with\\ncomparable \\\\textbf{state-of-the-art} methods\")\n",
    "def init_Quali():\n",
    "    global filter_cars, filter_ests\n",
    "    try:\n",
    "        for i in range(len(filter_cars)):\n",
    "            filter_cars[i].patch.set(visible = False)\n",
    "            filter_cars[i]._past.set(visible = False)\n",
    "    except Exception as e:\n",
    "        print(\"Failed to remove car collection\")\n",
    "        print(e)\n",
    "        pass\n",
    "    init_comment()\n",
    "    comment.set_text(\"We further summarise \\\\textbf{computation time}\\nand \\\\textbf{performance} of the different methods\\nover 5000 simulations\")\n",
    "def init_Quali2():\n",
    "    init_comment()\n",
    "    comment.set_text(\"The leftmost maroon method showcases\\nthe performance gain better approximations\\nof $a(e_k)$ and $b(x_k)$ could bring\")\n",
    "\n",
    "def make_show_comment(more_stay_frames=0):\n",
    "    def show_comment(frame):\n",
    "        if frame <= blend_in__frames:\n",
    "            comment.set_color([0,0,0,frame/blend_in__frames])\n",
    "        elif frame > blend_in__frames+stay_frames+more_stay_frames+1:\n",
    "            comment.set_color([0,0,0,(blend_out_frames-frame+blend_in__frames+stay_frames+more_stay_frames+1)/blend_out_frames])\n",
    "        pbar.update()\n",
    "    return show_comment\n",
    "def make_show_Title(more_stay_frames=0):\n",
    "    def show_Title(frame):\n",
    "        if frame <= blend_in__frames:\n",
    "            comment.set_color([0,0,0,frame/blend_in__frames])\n",
    "            authors.set_color([0,0,0,frame/blend_in__frames])\n",
    "        elif frame > blend_in__frames+stay_frames+more_stay_frames+1:\n",
    "            comment.set_color([0,0,0,(blend_out_frames-frame+blend_in__frames+stay_frames+more_stay_frames+1)/blend_out_frames])\n",
    "            authors.set_color([0,0,0,(blend_out_frames-frame+blend_in__frames+stay_frames+more_stay_frames+1)/blend_out_frames])\n",
    "        pbar.update()\n",
    "    return show_Title\n",
    "\n",
    "fig, ax  = plt.subplots(1, 1, figsize=(10,6))\n",
    "ax.set_xlim(xlimmin, xlimmax)\n",
    "ax.set_ylim(ylimmin, ylimmax)\n",
    "ax.set_aspect(\"equal\")\n",
    "ax.set_xticks([],[])\n",
    "ax.set_yticks([],[])\n",
    "ax_time, ax_error, ax_inv1, ax_inv2 = fig.add_subplot(5,2,(3,7)), fig.add_subplot(5,2,(4,8)), fig.add_subplot(2,2,1), fig.add_subplot(2,2,2)\n",
    "ax_inv1.set_visible(False); ax_inv2.set_visible(False)\n",
    "ax_time.set_xticks(np.arange(9),['our, oracle\\n$a(e)$, $b(x)$', 'our', 'Huang 2020', 'Agamennoni 2011', 'Chang 2014', 'Chang 2014', 'Särkkä 2012', 'Roth 2013', 'Kalman Filter'], rotation=-90)\n",
    "ax_error.set_xticklabels(['our, oracle\\n$a(e)$, $b(x)$', 'our', 'Huang 2020', 'Agamennoni 2011', 'Chang 2014', 'Chang 2014', 'Särkkä 2012', 'Roth 2013', 'Kalman Filter'], rotation=-90)\n",
    "\n",
    "comment = ax.text(x=midx, y=midy, ha=\"center\", va=\"center\", fontsize=22, s=\"\")\n",
    "authors = ax.text(x=midx, y=(midy+miny)/2, ha=\"center\", va=\"center\", fontsize=14, s=\"Jonas Neuhöfer \\\\hspace{10pt} Jörg Reichardt\")\n",
    "authors.set_color([0,0,0,0])\n",
    "init_Proposed3()\n",
    "#ax_time.set_visible(True); ax_error.set_visible(True)\n",
    "comment.set_color([0,0,0,1])\n",
    "plt.close(fig)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "for <class 'src.filters.proposed.StudentTFilter_GT'>: {}\n",
      "for <class 'src.filters.proposed.StudentTFilter'>: {'nu': 3.2644}\n",
      "for <class 'src.filters.robust.Huang_SSM'>: {'SSM': 'log', 'nu_SSM': 2.6022000000000003}\n",
      "for <class 'src.filters.robust.chang_RKF'>: {'alpha': 0.0883}\n",
      "for <class 'src.filters.robust.Agamennoni_VBF'>: {'nu': 12.3095}\n",
      "for <class 'src.filters.basic.KalmanFilter'>: {'gating': 0.9999}\n",
      "for <class 'src.filters.robust.chang_ARKF'>: {'alpha': 0.1764}\n",
      "for <class 'src.filters.robust.Saerkkae_VBF'>: {'rho': 0.0001}\n",
      "for <class 'src.filters.robust.roth_STF'>: {'obs_delta': 2.1737, 'process_gamma': 12.387500000000001, 'state_nu': 4.404100000000001}\n"
     ]
    }
   ],
   "source": [
    "# collect data of the quantitative analysis\n",
    "model_kwargs  = {\"alpha\":0.5, \"beta\":0.25, \"T\":500, \"R_nu\":1} # TODO: needs to be rerendered with the proper values\n",
    "best_results = eval.simulate.collect_best_results(model_kwargs, eval.simulate.Filters_To_Optimise)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Animating Multiple robust filters: 100%|██████████| 201/201 [00:17<00:00, 11.69it/s]\n",
      "Show qualitative results                      : : 5111it [05:02,  7.14it/s]                        "
     ]
    }
   ],
   "source": [
    "concatenate = [\n",
    "    (init_Title, make_show_Title(), (0, show_comment_frames), \n",
    "     \"Display title                                 \"),\n",
    "    (init_FreeAgent, make_show_comment(-30*upsampling_factor), (0, show_comment_frames-30*upsampling_factor), \n",
    "     \"Display setting                               \"),\n",
    "    (init1, animate_update1, (0,len(upsampledt)), \n",
    "     \"Animating only the agent                      \"),\n",
    "    (init_Tracking_Gaus, make_show_comment(), (0, show_comment_frames), \n",
    "     \"Display sensor problem                        \"),\n",
    "    (init2, make_animate_update2(upsampledy_Gaussian,upsampledt_y), (0,len(upsampledt)), \n",
    "     \"Animating the agent and Gaussian observations \"),\n",
    "    (init_KF_intro05, make_show_comment(), (0, show_comment_frames), \n",
    "     \"Display reconstruction task                   \"),\n",
    "    (init_KF_intro, make_show_comment(30*upsampling_factor), (0, show_comment_frames+30*upsampling_factor), \n",
    "     \"Display Kalman Filter intro                   \"),\n",
    "    (make_init3(-1), make_animate_update3(upsampledy_Gaussian,upsampledt_y,filters.basic.KalmanFilter), (0,len(upsampledt)), \n",
    "     \"Animating the Kalman Filter                   \"),\n",
    "    (init_KF_outliers, make_show_comment(30*upsampling_factor), (0, show_comment_frames+30*upsampling_factor), \n",
    "     \"Display real-worl measurements non-Gaussian   \"),\n",
    "    (make_init3(0), make_animate_update3(upsampledy,upsampledt_y,filters.basic.KalmanFilter), (0,len(upsampledt)), \n",
    "     \"Animating the agent and Student-t observations\"),\n",
    "    (init_Gating1, make_show_comment(30*upsampling_factor), (0, show_comment_frames+30*upsampling_factor), \n",
    "     \"Display gating explanation 1                  \"),\n",
    "    (init_Gating2, make_show_comment(15*upsampling_factor), (0, show_comment_frames+15*upsampling_factor), \n",
    "     \"Display gating explanation 2                  \"),\n",
    "    (init_Gating3, make_show_comment(), (0, show_comment_frames), \n",
    "     \"Display gating problem                        \"),\n",
    "    (make_init3(1), make_animate_update3(upsampledy,upsampledt_y,filters.basic.KalmanFilter, draw_ellipse_p=0.99, elongate=2, gating=0.99), (0,int(len(upsampledt)*2)), \n",
    "     \"Animating Gating                              \"),\n",
    "    (init_Proposed1, make_show_comment(30*upsampling_factor), (0, show_comment_frames+30*upsampling_factor), \n",
    "     \"Display prior work                            \"),\n",
    "    (init_Proposed2, make_show_comment(30*upsampling_factor), (0, show_comment_frames+30*upsampling_factor), \n",
    "     \"Display our approach                          \"),\n",
    "    (init_Proposed3, make_show_comment(30*upsampling_factor), (0, show_comment_frames+30*upsampling_factor), \n",
    "     \"Display equalities                            \"),\n",
    "    (init_Proposed4, make_show_comment(), (0, show_comment_frames), \n",
    "     \"Display local approximations                  \"),\n",
    "    (init_Proposed5, make_show_comment(), (0, show_comment_frames), \n",
    "     \"Display mathematical properties               \"),\n",
    "    (make_init3(2), make_animate_update3(upsampledy,upsampledt_y,filters.proposed.StudentTFilter, nu=3), (0,len(upsampledt)), \n",
    "     \"Animating our method                          \"),\n",
    "    (init_Multiple, make_show_comment(), (0, show_comment_frames), \n",
    "     \"Display qualitative comparison                \"),\n",
    "    (init_multiple_filters, make_animate_multiple_filters(), (0,len(upsampledt)), \n",
    "     \"Display comparison on familiar trajectory     \"),\n",
    "    (init_multiple_filters, make_animate_multiple_filters(), (0,int(len(upsampledt))), \n",
    "     \"Display comparison on trajectory 2            \"),\n",
    "#    (init_multiple_filters, make_animate_multiple_filters(), (0,int(len(upsampledt))), \n",
    "#     \"Display comparison on trajectory 3            \"),\n",
    "    (init_Quali, make_show_comment(30*upsampling_factor), (0, show_comment_frames+30*upsampling_factor), \n",
    "     \"Display comparison with other methods         \"),\n",
    "    (init_qualitative, empty, (0,len(upsampledt)+60*upsampling_factor), \n",
    "     \"Show qualitative results                      \"),\n",
    "    (init_Quali2, make_show_comment(), (0, show_comment_frames), \n",
    "     \"Display qualitative comparison                \"),\n",
    "    (init_qualitative, empty, (0,len(upsampledt)+60*upsampling_factor), \n",
    "     \"Show qualitative results                      \"),\n",
    "]\n",
    "tot_len = sum([c[2][1]-c[2][0]  for c in concatenate])\n",
    "pbar = tqdm(desc=\"\", total=tot_len, leave=None, position=0, file=sys.stdout)\n",
    "def animate_update_concatenate(frame):\n",
    "    i = 0\n",
    "    while frame >= concatenate[i][2][1]-concatenate[i][2][0]:\n",
    "        frame -= concatenate[i][2][1]-concatenate[i][2][0]\n",
    "        i += 1\n",
    "    if frame == 0:\n",
    "        concatenate[i][0]()\n",
    "        pbar.set_description(concatenate[i][3])\n",
    "    concatenate[i][1](frame+concatenate[i][2][0])\n",
    "\n",
    "fig, ax  = plt.subplots(1, 1, figsize=(10,6))\n",
    "ax.set_xlim(xlimmin, xlimmax)\n",
    "ax.set_ylim(ylimmin, ylimmax)\n",
    "ax.set_aspect(\"equal\")\n",
    "ax.set_xticks([],[])\n",
    "ax.set_yticks([],[])\n",
    "ax_time, ax_error, ax_inv1, ax_inv2 = fig.add_subplot(5,2,(3,7)), fig.add_subplot(5,2,(4,8)), fig.add_subplot(2,2,1), fig.add_subplot(2,2,2)\n",
    "ax_inv1.set_visible(False); ax_inv2.set_visible(False)\n",
    "ax_time.set_visible(False); ax_error.set_visible(False)\n",
    "eval.simulate.make_boxplot(results=best_results, ax=ax_time, categories=\"comp_time_filter\", maxboxoutlier=5)\n",
    "eval.simulate.make_boxplot(results=best_results, ax=ax_error, categories=\"euc_error\")\n",
    "ax_time.set_ylim(0, 0.5*ax_time.get_ylim()[1]); ax_error.set_ylim(0, ax_error.get_ylim()[1])\n",
    "ax_time.set_xticklabels(['our, oracle\\n$a(e)$, $b(x)$', 'our', 'Huang 2020', 'Chang 2014', 'Agamennoni 2011', 'gated\\nKalman Filter', 'Chang 2014', 'Särkkä 2012', 'Roth 2013'], rotation=-90)\n",
    "ax_error.set_xticklabels(['our, oracle\\n$a(e)$, $b(x)$', 'our', 'Huang 2020', 'Chang 2014', 'Agamennoni 2011', 'gated\\nKalman Filter', 'Chang 2014', 'Särkkä 2012', 'Roth 2013'], rotation=-90)\n",
    "\n",
    "car = visual.animations.Car(state=away_pos, ax=ax, model=model, length=carlength, past=0, predict=0, fc=\"#333333\", ec=\"none\")\n",
    "obserCar = visual.animations.Car(state=away_pos, ax=ax, model=model, length=carlength, past=0, predict=0, fc=\"blue\", ec=\"none\", alpha=0.4)\n",
    "filterCar= visual.animations.Car(state=away_pos, ax=ax, model=model, length=carlength, past=0, predict=0, fc=\"#9a9a9a\", ec=\"none\")#, alpha=0.4)\n",
    "obserArrow = ax.arrow(*away_pos[:4], width=car.length/4, head_length=car.length/2, head_width=car.length/2, fc=\"blue\", ec=\"none\", alpha=0.4)\n",
    "ellipse = ax.plot(away_pos[:2], color=\"red\", alpha=0.2)[0]\n",
    "cancelX = ax.plot([],[], \"x\", mfc=[0.66,0,0], mec=[0.66,0,0], markersize=carlength*7.5, alpha=0.5)[0]\n",
    "\n",
    "comment = ax.text(x=midx, y=midy, ha=\"center\", va=\"center\", fontsize=22, s=\"\")\n",
    "authors = ax.text(x=midx, y=(midy+miny)/2, ha=\"center\", va=\"center\", fontsize=14, s=\"Jonas Neuhöfer \\\\hspace{10pt} Jörg Reichardt\")\n",
    "authors.set_color([0,0,0,0])\n",
    "note = ax.text(x=maxx, y=maxy, ha=\"right\", va=\"top\", fontsize=14, s=f\"\")\n",
    "\n",
    "\n",
    "ani = animation.FuncAnimation(fig, func=animate_update_concatenate, frames=range(tot_len), repeat=True)\n",
    "Writer = animation.writers['ffmpeg']\n",
    "Writer = Writer(fps=30*upsampling_factor)\n",
    "ani.save(f\"../data/figures/AnimationExplainatory.mp4\", writer=Writer, dpi=200)\n",
    "plt.close(fig)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "STFenv",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.6"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}