KF.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# a multi-dimensional Kalman Filter\n",
    "\n",
    "from math import *\n",
    "\n",
    "class matrix:\n",
    "    \n",
    "    # implements basic operations of a matrix class\n",
    "    \n",
    "    def __init__(self, value):\n",
    "        self.value = value\n",
    "        self.dimx = len(value)\n",
    "        self.dimy = len(value[0])\n",
    "        if value == [[]]:\n",
    "            self.dimx = 0\n",
    "    \n",
    "    def zero(self, dimx, dimy):\n",
    "        # check if valid dimensions\n",
    "        if dimx < 1 or dimy < 1:\n",
    "            raise(ValueError, \"Invalid size of matrix\")\n",
    "        else:\n",
    "            self.dimx = dimx\n",
    "            self.dimy = dimy\n",
    "            self.value = [[0 for row in range(dimy)] for col in range(dimx)]\n",
    "    \n",
    "    def identity(self, dim):\n",
    "        # check if valid dimension\n",
    "        if dim < 1:\n",
    "            raise(ValueError, \"Invalid size of matrix\")\n",
    "        else:\n",
    "            self.dimx = dim\n",
    "            self.dimy = dim\n",
    "            self.value = [[0 for row in range(dim)] for col in range(dim)]\n",
    "            for i in range(dim):\n",
    "                self.value[i][i] = 1\n",
    "    \n",
    "    def show(self):\n",
    "        for i in range(self.dimx):\n",
    "            print(self.value[i])\n",
    "        print(' ')\n",
    "    \n",
    "    def __add__(self, other):\n",
    "        # check if correct dimensions\n",
    "        if self.dimx != other.dimx or self.dimy != other.dimy:\n",
    "            raise(ValueError, \"Matrices must be of equal dimensions to add\")\n",
    "        else:\n",
    "            # add if correct dimensions\n",
    "            res = matrix([[]])\n",
    "            res.zero(self.dimx, self.dimy)\n",
    "            for i in range(self.dimx):\n",
    "                for j in range(self.dimy):\n",
    "                    res.value[i][j] = self.value[i][j] + other.value[i][j]\n",
    "            return res\n",
    "    \n",
    "    def __sub__(self, other):\n",
    "        # check if correct dimensions\n",
    "        if self.dimx != other.dimx or self.dimy != other.dimy:\n",
    "            raise(ValueError, \"Matrices must be of equal dimensions to subtract\")\n",
    "        else:\n",
    "            # subtract if correct dimensions\n",
    "            res = matrix([[]])\n",
    "            res.zero(self.dimx, self.dimy)\n",
    "            for i in range(self.dimx):\n",
    "                for j in range(self.dimy):\n",
    "                    res.value[i][j] = self.value[i][j] - other.value[i][j]\n",
    "            return res\n",
    "    \n",
    "    def __mul__(self, other):\n",
    "        # check if correct dimensions\n",
    "        if self.dimy != other.dimx:\n",
    "            raise(ValueError, \"Matrices must be m*n and n*p to multiply\")\n",
    "        else:\n",
    "            # multiply if correct dimensions\n",
    "            res = matrix([[]])\n",
    "            res.zero(self.dimx, other.dimy)\n",
    "            for i in range(self.dimx):\n",
    "                for j in range(other.dimy):\n",
    "                    for k in range(self.dimy):\n",
    "                        res.value[i][j] += self.value[i][k] * other.value[k][j]\n",
    "            return res\n",
    "    \n",
    "    def transpose(self):\n",
    "        # compute transpose\n",
    "        res = matrix([[]])\n",
    "        res.zero(self.dimy, self.dimx)\n",
    "        for i in range(self.dimx):\n",
    "            for j in range(self.dimy):\n",
    "                res.value[j][i] = self.value[i][j]\n",
    "        return res\n",
    "    \n",
    "    # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions\n",
    "    \n",
    "    def Cholesky(self, ztol=1.0e-5):\n",
    "        # Computes the upper triangular Cholesky factorization of\n",
    "        # a positive definite matrix.\n",
    "        res = matrix([[]])\n",
    "        res.zero(self.dimx, self.dimx)\n",
    "        \n",
    "        for i in range(self.dimx):\n",
    "            S = sum([(res.value[k][i])**2 for k in range(i)])\n",
    "            d = self.value[i][i] - S\n",
    "            if abs(d) < ztol:\n",
    "                res.value[i][i] = 0.0\n",
    "            else:\n",
    "                if d < 0.0:\n",
    "                    raise(ValueError, \"Matrix not positive-definite\")\n",
    "                res.value[i][i] = sqrt(d)\n",
    "            for j in range(i+1, self.dimx):\n",
    "                S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)])\n",
    "                if abs(S) < ztol:\n",
    "                    S = 0.0\n",
    "                try:\n",
    "                    res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]\n",
    "                except:\n",
    "                    raise(ValueError, \"Zero diagonal\")\n",
    "        return res\n",
    "    \n",
    "    def CholeskyInverse(self):\n",
    "        # Computes inverse of matrix given its Cholesky upper Triangular\n",
    "        # decomposition of matrix.\n",
    "        res = matrix([[]])\n",
    "        res.zero(self.dimx, self.dimx)\n",
    "        \n",
    "        # Backward step for inverse.\n",
    "        for j in reversed(range(self.dimx)):\n",
    "            tjj = self.value[j][j]\n",
    "            S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])\n",
    "            res.value[j][j] = 1.0/tjj**2 - S/tjj\n",
    "            for i in reversed(range(j)):\n",
    "                res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]\n",
    "        return res\n",
    "    \n",
    "    def inverse(self):\n",
    "        aux = self.Cholesky()\n",
    "        res = aux.CholeskyInverse()\n",
    "        return res\n",
    "    \n",
    "    def __repr__(self):\n",
    "        return repr(self.value)\n",
    "    \n",
    "    \n",
    "def kalman_filter(x, P):\n",
    "    for n in range(len(measurements)):\n",
    "\n",
    "        # measurement update\n",
    "        z = matrix([[measurements[n]]])\n",
    "        y = z - (H * x)\n",
    "        S = H * P * H.transpose() + R\n",
    "        K = P * H.transpose() * S.inverse()\n",
    "        x = x + (K * y)\n",
    "        P = (I - (K * H)) * P\n",
    "        \n",
    "        # prediction\n",
    "        x = (F * x) + u\n",
    "        P = F * P * F.transpose()\n",
    "        \n",
    "    return x,P\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "([[3.9996664447958645], [0.9999998335552873]], [[2.3318904241194827, 0.9991676099921091], [0.9991676099921067, 0.49950058263974184]])\n"
     ]
    }
   ],
   "source": [
    "\n",
    "measurements = [1, 2, 3]\n",
    "\n",
    "x = matrix([[0.], [0.]]) # initial state (location and velocity)\n",
    "P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty\n",
    "u = matrix([[0.], [0.]]) # external motion\n",
    "F = matrix([[1., 1.], [0, 1.]]) # next state function\n",
    "H = matrix([[1., 0.]]) # measurement function\n",
    "R = matrix([[1.]]) # measurement uncertainty\n",
    "I = matrix([[1., 0.], [0., 1.]]) # identity matrix\n",
    "\n",
    "print(kalman_filter(x, P))\n",
    "# output should be:\n",
    "# x: [[3.9996664447958645], [0.9999998335552873]]\n",
    "# P: [[2.3318904241194827, 0.9991676099921091], [0.9991676099921067, 0.49950058263974184]]\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.6.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}