2. Kalman_Filter.ipynb

{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# This file is practice for the AI for Robotics course by Sebastian Thrun\n",
    "\n",
    "## Kalman Filters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[12.4, 1.6]\n"
     ]
    }
   ],
   "source": [
    "# Write a program to update your mean and variance\n",
    "# when given the mean and variance of your belief\n",
    "# and the mean and variance of your measurement.\n",
    "# This program will update the parameters of your\n",
    "# belief function.\n",
    "\n",
    "def update(mean1, var1, mean2, var2):\n",
    "    new_mean = ((var2 * mean1) + (var1 * mean2)) / (var2 + var1)\n",
    "    new_var = 1/((1/var2) + (1/var1))\n",
    "    return [new_mean, new_var]\n",
    "\n",
    "print (update(10.,8.,13., 2.))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[22.0, 8.0]\n"
     ]
    }
   ],
   "source": [
    "# Write a program that will predict your new mean\n",
    "# and variance given the mean and variance of your \n",
    "# prior belief and the mean and variance of your \n",
    "# motion. \n",
    "\n",
    "\n",
    "# mean 1 and var 1 are of the prior belief\n",
    "# mean 2 and var2 are of the measurement probability uncertainty\n",
    "# new_mean and new_var of of the posterior belief\n",
    "def update(mean1, var1, mean2, var2): \n",
    "    new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2)\n",
    "    new_var = 1/(1/var1 + 1/var2)\n",
    "    return [new_mean, new_var]\n",
    "\n",
    "# mean 1 and var 1 are of the prior belief\n",
    "# mean 2 and var2 are of the motion uncertainty\n",
    "# new_mean and new_var of of the posterior belief\n",
    "def predict(mean1, var1, mean2, var2):\n",
    "    new_mean = mean1 + mean2\n",
    "    new_var = var1 + var2\n",
    "    return [new_mean, new_var]\n",
    "\n",
    "print (predict(10., 4., 12., 4.))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[10.999906177177365, 4.005861580844194]\n"
     ]
    }
   ],
   "source": [
    "# Write a program that will iteratively update and\n",
    "# predict based on the location measurements \n",
    "# and inferred motions shown below. \n",
    "\n",
    "def update(mean1, var1, mean2, var2):\n",
    "    new_mean = float(var2 * mean1 + var1 * mean2) / (var1 + var2)\n",
    "    new_var = 1./(1./var1 + 1./var2)\n",
    "    return [new_mean, new_var]\n",
    "\n",
    "def predict(mean1, var1, mean2, var2):\n",
    "    new_mean = mean1 + mean2\n",
    "    new_var = var1 + var2\n",
    "    return [new_mean, new_var]\n",
    "\n",
    "measurements = [5., 6., 7., 9., 10.]  # means of measurement\n",
    "motion = [1., 1., 2., 1., 1.]  # means of motion\n",
    "measurement_sig = 4.  # uncertainties of both measurement(update) and motion(predict) remain same\n",
    "motion_sig = 2.\n",
    "mu = 0.\n",
    "sig = 10000.\n",
    "\n",
    "#Please print out ONLY the final values of the mean\n",
    "#and the variance in a list [mu, sig]. \n",
    "\n",
    "# Insert code here\n",
    "for k in range(len(measurements)):\n",
    "    mu, sig = update(mu, sig, measurements[k], measurement_sig)\n",
    "    mu, sig = predict(mu, sig, motion[k], motion_sig)\n",
    "\n",
    "\n",
    "print ([mu, sig]) # mean and variance of the posterior belief (next position and its variance)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Designing a Kalman Filter"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "([[3.9996664447958645], [0.9999998335552873]], [[2.3318904241194827, 0.9991676099921091], [0.9991676099921067, 0.49950058263974184]])\n"
     ]
    }
   ],
   "source": [
    "# Write a function 'kalman_filter' that implements a multi-\n",
    "# dimensional Kalman Filter for the example given\n",
    "\n",
    "from math import *\n",
    "\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",
    "########################################\n",
    "\n",
    "# Implement the filter function below\n",
    "\n",
    "def kalman_filter(x, P):\n",
    "    for n in range(len(measurements)):\n",
    "        \n",
    "        # measurement update\n",
    "        y = matrix.__sub__(matrix([[measurements[n]]]), matrix.__mul__(H, x))  # Since, measurement is just an int value, we convert it to a 2-D list and then into a matrix\n",
    "        S = matrix.__add__(matrix.__mul__(matrix.__mul__(H, P), matrix.transpose(H)), R)\n",
    "        K = matrix.__mul__(matrix.__mul__(P, matrix.transpose(H)), matrix.inverse(S))\n",
    "        x = matrix.__add__(x, matrix.__mul__(K, y))\n",
    "        P = matrix.__mul__(matrix.__sub__(I, matrix.__mul__(K, H)), P)\n",
    "\n",
    "        # prediction\n",
    "        x = matrix.__add__(matrix.__mul__(F, x), u)\n",
    "        P = matrix.__mul__(matrix.__mul__(F, P), matrix.transpose(F))\n",
    "        \n",
    "    return x,P\n",
    "\n",
    "############################################\n",
    "### use the code below to test your filter!\n",
    "############################################\n",
    "\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": 23,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "### 4-dimensional example ###\n",
      "x= \n",
      "[9.999340731787717]\n",
      "[0.001318536424568617]\n",
      "[9.998901219646193]\n",
      "[-19.997802439292386]\n",
      " \n",
      "P= \n",
      "[0.03955609273706198, 0.0, 0.06592682122843721, 0.0]\n",
      "[0.0, 0.03955609273706198, 0.0, 0.06592682122843721]\n",
      "[0.06592682122843718, 0.0, 0.10987803538073201, 0.0]\n",
      "[0.0, 0.06592682122843718, 0.0, 0.10987803538073201]\n",
      " \n"
     ]
    }
   ],
   "source": [
    "############ Programming Assignment ####################\n",
    "# Fill in the matrices P, F, H, R and I at the bottom\n",
    "#\n",
    "# This question requires NO CODING, just fill in the \n",
    "# matrices where indicated. Please do not delete or modify\n",
    "# any provided code OR comments. Good luck!\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",
    "            # subtract 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",
    "########################################\n",
    "\n",
    "def filter(x, P):\n",
    "    for n in range(len(measurements)):\n",
    "        \n",
    "        # prediction\n",
    "        x = (F * x) + u\n",
    "        P = F * P * F.transpose()\n",
    "        \n",
    "        # measurement update\n",
    "        Z = matrix([measurements[n]])\n",
    "        y = Z.transpose() - (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",
    "    print ('x= ')\n",
    "    x.show()\n",
    "    print ('P= ')\n",
    "    P.show()\n",
    "\n",
    "########################################\n",
    "\n",
    "print (\"### 4-dimensional example ###\")\n",
    "\n",
    "measurements = [[5., 10.], [6., 8.], [7., 6.], [8., 4.], [9., 2.], [10., 0.]]\n",
    "initial_xy = [4., 12.]\n",
    "\n",
    "# measurements = [[1., 4.], [6., 0.], [11., -4.], [16., -8.]]\n",
    "# initial_xy = [-4., 8.]\n",
    "\n",
    "# measurements = [[1., 17.], [1., 15.], [1., 13.], [1., 11.]]\n",
    "# initial_xy = [1., 19.]\n",
    "\n",
    "dt = 0.1\n",
    "\n",
    "x = matrix([[initial_xy[0]], [initial_xy[1]], [0.], [0.]]) # initial state (location and velocity)\n",
    "u = matrix([[0.], [0.], [0.], [0.]]) # external motion\n",
    "\n",
    "#### DO NOT MODIFY ANYTHING ABOVE HERE ####\n",
    "#### fill this in, remember to use the matrix() function!: ####\n",
    "\n",
    "P = matrix([[0, 0, 0, 0], [0., 0, 0, 0], [0, 0, 1000., 0], [0, 0, 0, 1000.]])# initial uncertainty: 0 for positions x and y, 1000 for the two velocities\n",
    "F = matrix([[1., 0, dt, 0], [0, 1., 0, dt], [0, 0, 1., 0], [0, 0, 0, 1.]]) # next state function: generalize the 2d version to 4d\n",
    "H = matrix([[1., 0., 0, 0], [0, 1, 0, 0]]) # measurement function: reflect the fact that we observe x and y but not the two velocities\n",
    "R = matrix([[0.1, 0], [0, 0.1]])# measurement uncertainty: use 2x2 matrix with 0.1 as main diagonal\n",
    "I = matrix([[1., 0., 0, 0], [0., 1., 0, 0], [0, 0, 1, 0], [0, 0, 0, 1.]]) # 4d identity matrix\n",
    "\n",
    "\n",
    "###### DO NOT MODIFY ANYTHING HERE #######\n",
    "\n",
    "filter(x, P)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "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.12"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}