{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "### Import standard libraries\n",
    "\n",
    "import abc\n",
    "from dataclasses import dataclass\n",
    "import functools\n",
    "from functools import partial\n",
    "import itertools\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "from typing import Any, Callable, NamedTuple, Optional, Union, Tuple\n",
    "\n",
    "import jax\n",
    "import jax.numpy as jnp\n",
    "from jax import lax, vmap, jit, grad\n",
    "#from jax.scipy.special import logit\n",
    "#from jax.nn import softmax\n",
    "import jax.random as jr\n",
    "\n",
    "\n",
    "\n",
    "import distrax\n",
    "import optax\n",
    "\n",
    "import jsl\n",
    "import ssm_jax\n",
    "\n",
    "import inspect\n",
    "import inspect as py_inspect\n",
    "import rich\n",
    "from rich import inspect as r_inspect\n",
    "from rich import print as r_print\n",
    "\n",
    "def print_source(fname):\n",
    "    r_print(py_inspect.getsource(fname))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "(sec:lds-intro)=\n",
    "# Linear Gaussian SSMs\n",
    "\n",
    "\n",
    "Consider the state space model in \n",
    "{eq}`eq:SSM-ar`\n",
    "where we assume the observations are conditionally iid given the\n",
    "hidden states and inputs (i.e. there are no auto-regressive dependencies\n",
    "between the observables).\n",
    "We can rewrite this model as \n",
    "a stochastic $\\keyword{nonlinear dynamical system}$ or $\\keyword{NLDS}$\n",
    "by defining the distribution of the next hidden state \n",
    "$\\hidden_t \\in \\real^{\\nhidden}$\n",
    "as a deterministic function of the past state\n",
    "$\\hidden_{t-1}$,\n",
    "the input $\\inputs_t \\in \\real^{\\ninputs}$,\n",
    "and some random $\\keyword{process noise}$ $\\transNoise_t \\in \\real^{\\nhidden}$ \n",
    "\\begin{align}\n",
    "\\hidden_t &= \\dynamicsFn(\\hidden_{t-1}, \\inputs_t, \\transNoise_t)  \n",
    "\\end{align}\n",
    "where $\\transNoise_t$ is drawn from the distribution such\n",
    "that the induced distribution\n",
    "on $\\hidden_t$ matches $p(\\hidden_t|\\hidden_{t-1}, \\inputs_t)$.\n",
    "Similarly we can rewrite the observation distribution\n",
    "as a deterministic function of the hidden state\n",
    "plus $\\keyword{observation noise}$ $\\obsNoise_t \\in \\real^{\\nobs}$:\n",
    "\\begin{align}\n",
    "\\obs_t &= \\measurementFn(\\hidden_{t}, \\inputs_t, \\obsNoise_t)\n",
    "\\end{align}\n",
    "\n",
    "\n",
    "If we assume additive Gaussian noise,\n",
    "the model becomes\n",
    "\\begin{align}\n",
    "\\hidden_t &= \\dynamicsFn(\\hidden_{t-1}, \\inputs_t) +  \\transNoise_t  \\\\\n",
    "\\obs_t &= \\measurementFn(\\hidden_{t}, \\inputs_t) + \\obsNoise_t\n",
    "\\end{align}\n",
    "where $\\transNoise_t \\sim \\gauss(\\vzero,\\transCov_t)$\n",
    "and $\\obsNoise_t \\sim \\gauss(\\vzero,\\obsCov_t)$.\n",
    "We will call these $\\keyword{Gaussian SSMs}$.\n",
    "\n",
    "If we additionally assume\n",
    "the transition function $\\dynamicsFn$\n",
    "and the observation function $\\measurementFn$ are both linear,\n",
    "then we can rewrite the model as follows:\n",
    "\\begin{align}\n",
    "p(\\hidden_t|\\hidden_{t-1},\\inputs_t) &= \\gauss(\\hidden_t|\\ldsDyn \\hidden_{t-1}\n",
    "+ \\ldsDynIn \\inputs_t, \\transCov)\n",
    "\\\\\n",
    "p(\\obs_t|\\hidden_t,\\inputs_t) &= \\gauss(\\obs_t|\\ldsObs \\hidden_{t}\n",
    "+ \\ldsObsIn \\inputs_t, \\obsCov)\n",
    "\\end{align}\n",
    "This is called a \n",
    "$\\keyword{linear-Gaussian state space model}$\n",
    "or $\\keyword{LG-SSM}$;\n",
    "it is also called \n",
    "a $\\keyword{linear dynamical system}$ or $\\keyword{LDS}$.\n",
    "We usually assume the parameters are independent of time, in which case\n",
    "the model is said to be time-invariant or homogeneous.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "(sec:tracking-lds)=\n",
    "(sec:kalman-tracking)=\n",
    "## Example: tracking a 2d point\n",
    "\n",
    "\n",
    "\n",
    "% Sarkkar p43\n",
    "Consider an object moving in $\\real^2$.\n",
    "Let the state be\n",
    "the position and velocity of the object,\n",
    "$\\hidden_t =\\begin{pmatrix} u_t & \\dot{u}_t & v_t & \\dot{v}_t \\end{pmatrix}$.\n",
    "(We use $u$ and $v$ for the two coordinates,\n",
    "to avoid confusion with the state and observation variables.)\n",
    "If we use Euler discretization,\n",
    "the dynamics become\n",
    "\\begin{align}\n",
    "\\underbrace{\\begin{pmatrix} u_t\\\\ \\dot{u}_t \\\\ v_t \\\\ \\dot{v}_t \\end{pmatrix}}_{\\hidden_t}\n",
    "  = \n",
    "\\underbrace{\n",
    "\\begin{pmatrix}\n",
    "1 & 0 & \\Delta & 0 \\\\\n",
    "0 & 1 & 0 & \\Delta\\\\\n",
    "0 & 0 & 1 & 0 \\\\\n",
    "0 & 0 & 0 & 1\n",
    "\\end{pmatrix}\n",
    "}_{\\ldsDyn}\n",
    "\n",
    "\\underbrace{\\begin{pmatrix} u_{t-1} \\\\ \\dot{u}_{t-1} \\\\ v_{t-1} \\\\ \\dot{v}_{t-1} \\end{pmatrix}}_{\\hidden_{t-1}}\n",
    "+ \\transNoise_t\n",
    "\\end{align}\n",
    "where $\\transNoise_t \\sim \\gauss(\\vzero,\\transCov)$ is\n",
    "the process noise.\n",
    "We assume\n",
    "that the process noise is \n",
    "a white noise process added to the velocity components\n",
    "of the state, but not to the location,\n",
    "so $\\transCov = \\diag(0, q, 0, q)$.\n",
    "This is known as a random accelerations model.\n",
    "(See  {cite}`Sarkka13` p60 for a more accurate way\n",
    "to convert the continuous time process to discrete time.)\n",
    "\n",
    "\n",
    "Now suppose that at each discrete time point we\n",
    "observe the location,\n",
    "corrupted by  Gaussian noise.\n",
    "Thus the observation model becomes\n",
    "\\begin{align}\n",
    "\\underbrace{\\begin{pmatrix}  \\obs_{1,t} \\\\  \\obs_{2,t} \\end{pmatrix}}_{\\obs_t}\n",
    "  &=\n",
    "    \\underbrace{\n",
    "    \\begin{pmatrix}\n",
    "1 & 0 & 0 & 0 \\\\\n",
    "0 & 0 & 1 & 0\n",
    "    \\end{pmatrix}\n",
    "    }_{\\ldsObs}\n",
    "    \n",
    "\\underbrace{\\begin{pmatrix} u_t\\\\ \\dot{u}_t \\\\ v_t \\\\ \\dot{v}_t \\end{pmatrix}}_{\\hidden_t}    \n",
    " + \\obsNoise_t\n",
    "\\end{align}\n",
    "where $\\obsNoise_t \\sim \\gauss(\\vzero,\\obsCov)$ is the observation noise.\n",
    "We see that the observation matrix $\\ldsObs$ simply ``extracts'' the\n",
    "relevant parts  of the state vector.\n",
    "\n",
    "Suppose we sample a trajectory and corresponding set\n",
    "of noisy observations from this model,\n",
    "$(\\hidden_{1:T}, \\obs_{1:T}) \\sim p(\\hidden,\\obs|\\params)$.\n",
    "(We use diagonal observation noise,\n",
    "$\\obsCov = \\diag(\\sigma_1^2, \\sigma_2^2)$.)\n",
    "The results are shown below. \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "LDS(A=DeviceArray([[1., 0., 1., 0.],\n",
      "             [0., 1., 0., 1.],\n",
      "             [0., 0., 1., 0.],\n",
      "             [0., 0., 0., 1.]], dtype=float32), C=DeviceArray([[1, 0, 0, 0],\n",
      "             [0, 1, 0, 0]], dtype=int32), Q=DeviceArray([[0.001, 0.   , 0.   , 0.   ],\n",
      "             [0.   , 0.001, 0.   , 0.   ],\n",
      "             [0.   , 0.   , 0.001, 0.   ],\n",
      "             [0.   , 0.   , 0.   , 0.001]], dtype=float32), R=DeviceArray([[1., 0.],\n",
      "             [0., 1.]], dtype=float32), mu=DeviceArray([ 8., 10.,  1.,  0.], dtype=float32), Sigma=DeviceArray([[1., 0., 0., 0.],\n",
      "             [0., 1., 0., 0.],\n",
      "             [0., 0., 1., 0.],\n",
      "             [0., 0., 0., 1.]], dtype=float32), state_offset=None, obs_offset=None, nstates=4, nobs=2)\n"
     ]
    }
   ],
   "source": [
    "key = jax.random.PRNGKey(314)\n",
    "timesteps = 15\n",
    "delta = 1.0\n",
    "A = jnp.array([\n",
    "    [1, 0, delta, 0],\n",
    "    [0, 1, 0, delta],\n",
    "    [0, 0, 1, 0],\n",
    "    [0, 0, 0, 1]\n",
    "])\n",
    "\n",
    "C = jnp.array([\n",
    "    [1, 0, 0, 0],\n",
    "    [0, 1, 0, 0]\n",
    "])\n",
    "\n",
    "state_size, _ = A.shape\n",
    "observation_size, _ = C.shape\n",
    "\n",
    "Q = jnp.eye(state_size) * 0.001\n",
    "R = jnp.eye(observation_size) * 1.0\n",
    "# Prior parameter distribution\n",
    "mu0 = jnp.array([8, 10, 1, 0]).astype(float)\n",
    "Sigma0 = jnp.eye(state_size) * 1.0\n",
    "\n",
    "from jsl.lds.kalman_filter import LDS, smooth, filter\n",
    "\n",
    "lds = LDS(A, C, Q, R, mu0, Sigma0)\n",
    "print(lds)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [],
   "source": [
    "from jsl.demos.plot_utils import plot_ellipse\n",
    "\n",
    "def plot_tracking_values(observed, filtered, cov_hist, signal_label, ax):\n",
    "    timesteps, _ = observed.shape\n",
    "    ax.plot(observed[:, 0], observed[:, 1], marker=\"o\", linewidth=0,\n",
    "            markerfacecolor=\"none\", markeredgewidth=2, markersize=8, label=\"observed\", c=\"tab:green\")\n",
    "    ax.plot(*filtered[:, :2].T, label=signal_label, c=\"tab:red\", marker=\"x\", linewidth=2)\n",
    "    for t in range(0, timesteps, 1):\n",
    "        covn = cov_hist[t][:2, :2]\n",
    "        plot_ellipse(covn, filtered[t, :2], ax, n_std=2.0, plot_center=False)\n",
    "    ax.axis(\"equal\")\n",
    "    ax.legend()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(7.24486608505249, 23.857812213897706, 8.0420747756958, 11.636079216003418)"
      ]
     },
     "execution_count": 32,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "[encoded data, 10392 characters]",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "\n",
    "z_hist, x_hist = lds.sample(key, timesteps)\n",
    "\n",
    "fig_truth, axs = plt.subplots()\n",
    "axs.plot(x_hist[:, 0], x_hist[:, 1],\n",
    "        marker=\"o\", linewidth=0, markerfacecolor=\"none\",\n",
    "        markeredgewidth=2, markersize=8,\n",
    "        label=\"observed\", c=\"tab:green\")\n",
    "\n",
    "axs.plot(z_hist[:, 0], z_hist[:, 1],\n",
    "        linewidth=2, label=\"truth\",\n",
    "        marker=\"s\", markersize=8)\n",
    "axs.legend()\n",
    "axs.axis(\"equal\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The main task is to infer the hidden states given the noisy\n",
    "observations, i.e., $p(\\hidden_t|\\obs_{1:t},\\params)$\n",
    "or $p(\\hidden_t|\\obs_{1:T}, \\params)$ in the offline case.\n",
    "We discuss the topic of inference in {ref}`sec:inference`.\n",
    "We will usually represent this belief state by a Gaussian distribution,\n",
    "$p(\\hidden_t|\\obs_{1:s},\\params) = \\gauss(\\hidden_t| \\mean_{t|s}, \\covMat_{t|s})$,\n",
    "where usually $s=t$ or $s=T$.\n",
    "Sometimes we use information form, \n",
    "$p(\\hidden_t|\\obs_{1:s},\\params) = \\gaussInfo(\\hidden_t|\\precMean_{t|s}, \\precMat_{t|s})$."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.9.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
