Linear_Kalman_filter.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from numpy.random import normal\n",
    "import numpy as np\n",
    "from numpy.linalg import inv\n",
    "\n",
    "np.random.seed(0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Model 1: linear model\n",
      "x_k+1 = 2.0 * x_k + 0 + gaussian noise (mean 0.0,variance 5.0)\n",
      "y_k = 1.0 * x_k + gaussian noise  (mean 0.0,variance 5.0)\n",
      "\n",
      "[[  1.76405235e+00]\n",
      " [  5.52889073e+00]\n",
      " [  1.59514714e+01]\n",
      " [  4.31074088e+01]\n",
      " [  9.55526075e+01]\n",
      " [  1.86218826e+02]\n",
      " [  3.77188093e+02]\n",
      " [  7.53619400e+02]\n",
      " [  1.50672271e+03]\n",
      " [  3.01549841e+03]]\n"
     ]
    }
   ],
   "source": [
    "#Create the model\n",
    "T = 10\n",
    "\n",
    "#Model 1: linear model\n",
    "#x_k+1 = 2*x_k + 0 + gaussian noise (mean 0,variance 0.5)\n",
    "#y_k = x_k + gaussian noise  (mean 0,variance 0.5)\n",
    "A = np.identity(1) * 2\n",
    "B = np.zeros([1,1])\n",
    "C = np.identity(1)\n",
    "D = np.zeros([1,1])\n",
    "Q = np.identity(1) * 5\n",
    "R = np.identity(1) * 5\n",
    "mean = np.zeros([1,1])\n",
    "\n",
    "x = np.zeros([T,1])\n",
    "y = np.zeros([T,1])\n",
    "u = np.zeros([T,1])\n",
    "\n",
    "print(\"Model 1: linear model\"+ '\\n' + \\\n",
    "\"x_k+1 = \"+ str(A.item()) +\" * x_k + 0 + gaussian noise (mean \" + str(mean.item()) +\",variance \"+ str(Q.item()) +\")\"+ '\\n'+ \\\n",
    "\"y_k = \"+ str(C.item()) +\" * x_k + gaussian noise  (mean \"+ str(mean.item()) +\",variance \"+ str(R.item()) +\")\"+ '\\n')\n",
    "\n",
    "#Initialization\n",
    "# for k in range(T-1):\n",
    "#     u[k] = 0\n",
    "x[0] = normal(0,1)\n",
    "\n",
    "#Propagation\n",
    "for k in range(T-1):\n",
    "    x[k+1] = np.dot(A,x[k]) + np.dot(B,u[k]) + normal(mean,Q)\n",
    "for k in range(T):\n",
    "    y[k] = np.dot(C,x[k]) + np.dot(D,u[k]) + normal(mean,R)\n",
    "\n",
    "print(x)\n",
    "# print(y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ -2.55298982e+00]\n",
      " [  6.40517330e+00]\n",
      " [  1.82371540e+01]\n",
      " [  4.23028129e+01]\n",
      " [  9.52494087e+01]\n",
      " [  1.88386203e+02]\n",
      " [  3.83152279e+02]\n",
      " [  7.55212193e+02]\n",
      " [  1.50869605e+03]\n",
      " [  3.01240518e+03]]\n",
      "\n",
      "\n",
      "[[-4.31704216]\n",
      " [ 0.87628257]\n",
      " [ 2.28568257]\n",
      " [-0.80459583]\n",
      " [-0.30319883]\n",
      " [ 2.16737762]\n",
      " [ 5.96418594]\n",
      " [ 1.59279279]\n",
      " [ 1.97334398]\n",
      " [-3.09322594]]\n"
     ]
    }
   ],
   "source": [
    "#Kalman filter\n",
    "#from page 25 of ftp://icf.org.ru/pub/docs/linux-support/computer%20science/Artificial%20Intelligence/Neural%20networks/Kalman%20Filtering%20and%20Neural%20Networks%20-%20Simon%20Haykin.pdf\n",
    "\n",
    "#Initialization\n",
    "V = np.zeros([T,1])\n",
    "V[0] = np.identity(1)\n",
    "x_f = np.zeros([T,1])\n",
    "x_f[0] = normal(0,V[0])\n",
    "V_plus_all = np.zeros([T,1])\n",
    "x_plus_all = np.zeros([T,1])\n",
    "\n",
    "#Propagation\n",
    "for k in range(1,T):\n",
    "    x_plus = np.dot(A, x_f[k-1])\n",
    "    x_plus_all[k] = x_plus\n",
    "    V_plus = A.dot(V[k-1]).dot(A.T) + Q \n",
    "    V_plus_all[k] = V_plus\n",
    "    \n",
    "    K = V_plus.dot(C.T).dot(inv(C.dot(V_plus.dot(C.T) + R)))\n",
    "    x_f[k] = x_plus + K.dot(y[k] - C.dot(x_plus))\n",
    "    V[k] = (np.identity(K.shape[0]) - K.dot(C)).dot(V_plus)\n",
    "    \n",
    "print(x_f)\n",
    "print('\\n')\n",
    "print(x_f - x)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "9\n",
      "8\n",
      "7\n",
      "6\n",
      "5\n",
      "4\n",
      "3\n",
      "2\n",
      "1\n",
      "[[  7.32252339e-01]\n",
      " [  9.67761006e+00]\n",
      " [  2.19004487e+01]\n",
      " [  4.61454061e+01]\n",
      " [  9.46778776e+01]\n",
      " [  1.89002265e+02]\n",
      " [  3.78385319e+02]\n",
      " [  7.53824447e+02]\n",
      " [  1.50679122e+03]\n",
      " [  3.01240518e+03]]\n",
      "\n",
      "\n",
      "x_f - x_b =  \n",
      " [[-3.28524215]\n",
      " [-3.27243676]\n",
      " [-3.66329476]\n",
      " [-3.84259315]\n",
      " [ 0.57153104]\n",
      " [-0.61606173]\n",
      " [ 4.76696061]\n",
      " [ 1.38774676]\n",
      " [ 1.90483432]\n",
      " [ 0.        ]]\n",
      "\n",
      "x_f - x =  \n",
      " [[-4.31704216]\n",
      " [ 0.87628257]\n",
      " [ 2.28568257]\n",
      " [-0.80459583]\n",
      " [-0.30319883]\n",
      " [ 2.16737762]\n",
      " [ 5.96418594]\n",
      " [ 1.59279279]\n",
      " [ 1.97334398]\n",
      " [-3.09322594]]\n",
      "\n",
      "x_b - x =  \n",
      " [[-1.03180001]\n",
      " [ 4.14871933]\n",
      " [ 5.94897733]\n",
      " [ 3.03799732]\n",
      " [-0.87472987]\n",
      " [ 2.78343935]\n",
      " [ 1.19722533]\n",
      " [ 0.20504603]\n",
      " [ 0.06850966]\n",
      " [-3.09322594]]\n"
     ]
    }
   ],
   "source": [
    "#Recursive\n",
    "#F = A, B = H, P = V\n",
    "\n",
    "#Initialization\n",
    "x_b = np.zeros([T,1])\n",
    "x_b[-1] = x_f[-1]\n",
    "V_b = np.zeros([T,1])\n",
    "V_b[-1] = V[-1]\n",
    "\n",
    "#Propagation\n",
    "for k in range(T-1, 0, -1): #from T-1 to 1 by -1 increments\n",
    "    print(k)\n",
    "    A_b = V[k-1].dot(A.T).dot(inv(np.matrix(V_plus_all[k])))\n",
    "    V_b[k-1] = V[k-1] - A_b.dot(V_plus_all[k] - V_b[k]).dot(A_b.T)\n",
    "    x_b[k-1] = x_f[k-1] + A_b.dot(x_b[k] - x_plus_all[k])\n",
    "    \n",
    "print(x_b)   \n",
    "print('\\n')\n",
    "print(\"x_f - x_b = \",'\\n',x_f - x_b)\n",
    "print()\n",
    "print(\"x_f - x = \",'\\n',x_f - x)\n",
    "print()\n",
    "print(\"x_b - x = \",'\\n',x_b - x)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We get 0, as Kalman filter does all the job on linear systems (we can use the backward recursion, but it helps only for non-linear systems)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "# Now, take variable transition matrix (A_k and C_k, not A and C anymore)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[  1.76405235e+00]\n",
      " [  3.76483839e+00]\n",
      " [  1.24233667e+01]\n",
      " [  4.84745661e+01]\n",
      " [  2.03236054e+02]\n",
      " [  1.01129388e+03]\n",
      " [  6.07251373e+03]\n",
      " [  4.25068394e+04]\n",
      " [  3.40054199e+05]\n",
      " [  3.06048984e+06]]\n"
     ]
    }
   ],
   "source": [
    "#Create the model\n",
    "T = 10\n",
    "np.random.seed(0)\n",
    "#Model 2: linear model with dependence over time\n",
    "#x_k+1 = (k+1)*x_k + gaussian noise (mean 0,variance 1)\n",
    "#y_k = (1 + 1/k)*x_k + gaussian noise  (mean 0,variance 0.1)\n",
    "A = np.identity(1)\n",
    "A_all = [A*k for k in range(1,T+1)]  #A_all is such that x[k+1] = np.dot(A_all[k],x[k]) + ...etc\n",
    "B = np.zeros([1,1])\n",
    "C_all = np.zeros([T,1]) #C_all is such that y[k] = np.dot(C_all[k],x[k]) + ...etc\n",
    "for k in range(1,T):\n",
    "    C = np.identity(1)\n",
    "    C_all[k] = C*(1 + 1/(k+1))\n",
    "D = np.zeros([1,1])\n",
    "Q = np.identity(1) * 5\n",
    "R = np.identity(1) * 5\n",
    "mean = np.zeros([1,1])\n",
    "\n",
    "x = np.zeros([T,1])\n",
    "y = np.zeros([T,1])\n",
    "u = np.zeros([T,1])\n",
    "\n",
    "#Initialization\n",
    "# for k in range(T-1):\n",
    "#     u[k] = 0\n",
    "x[0] = normal(0,1)\n",
    "\n",
    "#Propagation\n",
    "for k in range(T-1):\n",
    "    x[k+1] = np.dot(A_all[k],x[k]) + np.dot(B,u[k]) + normal(mean,Q)\n",
    "for k in range(T):\n",
    "    y[k] = np.dot(C_all[k],x[k]) + np.dot(D,u[k]) + normal(mean,R)\n",
    "\n",
    "print(x)\n",
    "# print(y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "x_plus [ 1.]\n",
      "[ 1.5]\n",
      "h 1.5\n",
      "V_plus [[ 6.]]\n",
      "K [[ 0.42857143]]\n",
      "x_f[k] [ 5.89369648]\n",
      "V[k] [ 2.14285714]\n",
      "\n",
      "x_plus [ 11.78739296]\n",
      "[ 1.33333333]\n",
      "h 15.7165239427\n",
      "V_plus [[ 13.57142857]]\n",
      "K [[ 0.58762887]]\n",
      "x_f[k] [ 14.52172034]\n",
      "V[k] [ 2.93814433]\n",
      "\n",
      "x_plus [ 43.56516101]\n",
      "[ 1.25]\n",
      "h 54.456451263\n",
      "V_plus [[ 31.44329897]]\n",
      "K [[ 0.70971495]]\n",
      "x_f[k] [ 48.35228163]\n",
      "V[k] [ 3.54857475]\n",
      "\n",
      "x_plus [ 193.4091265]\n",
      "[ 1.2]\n",
      "h 232.090951806\n",
      "V_plus [[ 61.77719604]]\n",
      "K [[ 0.78067912]]\n",
      "x_f[k] [ 204.34771309]\n",
      "V[k] [ 3.9033956]\n",
      "\n",
      "x_plus [ 1021.73856547]\n",
      "[ 1.16666667]\n",
      "h 1192.02832638\n",
      "V_plus [[ 102.58488999]]\n",
      "K [[ 0.8227698]]\n",
      "x_f[k] [ 1013.08541949]\n",
      "V[k] [ 4.113849]\n",
      "\n",
      "x_plus [ 6078.51251695]\n",
      "[ 1.14285714]\n",
      "h 6946.87144794\n",
      "V_plus [[ 153.098564]]\n",
      "K [[ 0.85069036]]\n",
      "x_f[k] [ 6079.03538852]\n",
      "V[k] [ 4.25345182]\n",
      "\n",
      "x_plus [ 42553.24771965]\n",
      "[ 1.125]\n",
      "h 47872.4036846\n",
      "V_plus [[ 213.41913921]]\n",
      "K [[ 0.87075544]]\n",
      "x_f[k] [ 42506.89287746]\n",
      "V[k] [ 4.35377722]\n",
      "\n",
      "x_plus [ 340055.14301972]\n",
      "[ 1.11111111]\n",
      "h 377839.0478\n",
      "V_plus [[ 283.64174209]]\n",
      "K [[ 0.88594442]]\n",
      "x_f[k] [ 340055.60029067]\n",
      "V[k] [ 4.42972209]\n",
      "\n",
      "x_plus [ 3060500.40261607]\n",
      "[ 1.1]\n",
      "h 3366550.44288\n",
      "V_plus [[ 363.80748913]]\n",
      "K [[ 0.89787278]]\n",
      "x_f[k] [ 3060486.13763349]\n",
      "V[k] [ 4.48936389]\n",
      "\n",
      "[[  1.00000000e+00]\n",
      " [  5.89369648e+00]\n",
      " [  1.45217203e+01]\n",
      " [  4.83522816e+01]\n",
      " [  2.04347713e+02]\n",
      " [  1.01308542e+03]\n",
      " [  6.07903539e+03]\n",
      " [  4.25068929e+04]\n",
      " [  3.40055600e+05]\n",
      " [  3.06048614e+06]]\n",
      "\n",
      "\n",
      "[[-0.76405235]\n",
      " [ 2.12885809]\n",
      " [ 2.09835364]\n",
      " [-0.12228446]\n",
      " [ 1.11165881]\n",
      " [ 1.79153745]\n",
      " [ 6.52165418]\n",
      " [ 0.0535231 ]\n",
      " [ 1.40155002]\n",
      " [-3.70402495]]\n"
     ]
    }
   ],
   "source": [
    "#Kalman filter\n",
    "#from page 25 of ftp://icf.org.ru/pub/docs/linux-support/computer%20science/Artificial%20Intelligence/Neural%20networks/Kalman%20Filtering%20and%20Neural%20Networks%20-%20Simon%20Haykin.pdf\n",
    "\n",
    "#Initialization\n",
    "V = np.zeros([T,1])\n",
    "V[0] = np.identity(1)\n",
    "x_f = np.zeros([T,1])\n",
    "# x_f[0] = normal(0,V[0])\n",
    "x_f[0] = 1.\n",
    "V_plus_all = np.zeros([T,1])\n",
    "x_plus_all = np.zeros([T,1])\n",
    "\n",
    "#Propagation\n",
    "for k in range(1,T):\n",
    "    x_plus = np.dot(A_all[k-1], x_f[k-1])\n",
    "    x_plus_all[k] = x_plus\n",
    "    V_plus = A_all[k-1].dot(V[k-1]).dot(A_all[k-1].T) + Q \n",
    "    V_plus_all[k] = V_plus\n",
    "    \n",
    "    K = V_plus.dot(C_all[k].T).dot(inv(C_all[k].dot(np.matrix(V_plus.dot(C_all[k].T) + R))))\n",
    "    x_f[k] = x_plus + K.dot(y[k] - C_all[k].dot(x_plus))\n",
    "    V[k] = (np.identity(K.shape[0]) - K.dot(C_all[k])).dot(V_plus)\n",
    "    \n",
    "    print(\"x_plus\",x_plus)\n",
    "    print(C_all[k])\n",
    "    print(\"h\",C_all[k].dot(x_plus))\n",
    "#     print(\"A_all\",A_all)\n",
    "#     print(\"C_all\",C_all)\n",
    "    print(\"V_plus\",V_plus)\n",
    "    print(\"K\",K)\n",
    "    print(\"x_f[k]\",x_f[k])\n",
    "    print(\"V[k]\",V[k])\n",
    "    print()\n",
    "    \n",
    "print(x_f)\n",
    "print('\\n')\n",
    "print(x_f - x)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[  3.49239046e+00]\n",
      " [  8.47717137e+00]\n",
      " [  1.72413955e+01]\n",
      " [  5.08414849e+01]\n",
      " [  2.02076043e+02]\n",
      " [  1.01178830e+03]\n",
      " [  6.07161637e+03]\n",
      " [  4.25067161e+04]\n",
      " [  3.40053863e+05]\n",
      " [  3.06048614e+06]]\n",
      "\n",
      "\n",
      "x_f - x_b =  \n",
      " [[-2.49239046]\n",
      " [-2.58347489]\n",
      " [-2.71967518]\n",
      " [-2.48920332]\n",
      " [ 2.27167031]\n",
      " [ 1.29712435]\n",
      " [ 7.41901588]\n",
      " [ 0.17677644]\n",
      " [ 1.7369051 ]\n",
      " [ 0.        ]]\n",
      "\n",
      "x_f - x =  \n",
      " [[-0.76405235]\n",
      " [ 2.12885809]\n",
      " [ 2.09835364]\n",
      " [-0.12228446]\n",
      " [ 1.11165881]\n",
      " [ 1.79153745]\n",
      " [ 6.52165418]\n",
      " [ 0.0535231 ]\n",
      " [ 1.40155002]\n",
      " [-3.70402495]]\n",
      "\n",
      "x_b - x =  \n",
      " [[ 1.72833811]\n",
      " [ 4.71233299]\n",
      " [ 4.81802882]\n",
      " [ 2.36691886]\n",
      " [-1.1600115 ]\n",
      " [ 0.4944131 ]\n",
      " [-0.89736171]\n",
      " [-0.12325334]\n",
      " [-0.33535509]\n",
      " [-3.70402495]]\n"
     ]
    }
   ],
   "source": [
    "#Recursive\n",
    "#F = A, B = H, P = V\n",
    "\n",
    "#Initialization\n",
    "x_b = np.zeros([T,1])\n",
    "x_b[T-1] = x_f[T-1]\n",
    "V_b = np.zeros([T,1])\n",
    "V_b[T-1] = V[T-1]\n",
    "# print(len(A_all))\n",
    "\n",
    "#Propagation\n",
    "for k in range(T-1, 0, -1): #from T-1 to 1 by -1 increments\n",
    "    A_b = V[k-1].dot(A_all[k].T).dot(inv(np.matrix(V_plus_all[k])))\n",
    "    V_b[k-1] = V[k-1] - A_b.dot(V_plus_all[k] - V_b[k]).dot(A_b.T)\n",
    "    x_b[k-1] = x_f[k-1] + A_b.dot(x_b[k] - x_plus_all[k])\n",
    "    \n",
    "print(x_b)   \n",
    "print('\\n')\n",
    "print(\"x_f - x_b = \",'\\n',x_f - x_b)\n",
    "print()\n",
    "print(\"x_f - x = \",'\\n',x_f - x)\n",
    "print()\n",
    "print(\"x_b - x = \",'\\n',x_b - x)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "kernelspec": {
   "display_name": "Python [conda root]",
   "language": "python",
   "name": "conda-root-py"
  },
  "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.5.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}