Updated slides for lecture 3 and added exercise skeletons

Exercise/EKF_UKF/EKF.m

+function xhat = EKF(model,init,data)
+
+  M = length(data.y);
+  P = init.P0;
+  n = numel(init.x0);
+  
+  xhat = zeros(n,M); xhat(:,1) = init.x0;
+  
+  for l=1:M-1
+    % Measurement update
+    xhat(:,l) = FILL_IN_CODE_HERE;
+
+    % Time update
+    xhat(:,l+1) = FILL_IN_CODE_HERE;
+  end
+end
\ No newline at end of file

Exercise/EKF_UKF/task1.ipynb

+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 16,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import numpy as np\n",
+    "import matplotlib.pyplot as plt\n",
+    "import seaborn as sns\n",
+    "import pandas as pd"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 12,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "Using matplotlib backend: Qt5Agg\n"
+     ]
+    }
+   ],
+   "source": [
+    "%matplotlib\n",
+    "sns.set(style='white', rc={'lines.linewidth': 0.75})"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Simulate robot movement"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Define discrete-time model"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 9,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "Tfinal = 20\n",
+    "Ts = 0.1\n",
+    "V = 3\n",
+    "x0 = [10, 10, 0]  # Initial state\n",
+    "model = {}\n",
+    "model['f'] = lambda x, u, w: [x[0] + Ts*V*np.cos(x[2]), x[1] + Ts*V*np.sin(x[2]), x[2] + Ts*u]\n",
+    "model['h'] = lambda x, u: np.array([np.sqrt(x[0]**2 + x[1]**2), np.arctan2(x[1], x[0])])"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Simulate robot movement and store results in a dictionary"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 21,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "t = np.arange(0, Tfinal, Ts)\n",
+    "N = len(t)\n",
+    "x = np.zeros((3, N))\n",
+    "x[:, 0] = x0\n",
+    "\n",
+    "u = np.zeros((1, N))\n",
+    "u[0][np.logical_and(t >= 5, t < 10)] = -1\n",
+    "u[0][np.logical_and(t >= 10, t < 12)] = 0\n",
+    "u[0][np.logical_and(t >= 12, t < 15)] = 1\n",
+    "u[0][np.logical_and(t >= 15, t < 17)] = -1\n",
+    "\n",
+    "for ti in np.arange(0, N-1):\n",
+    "  x[:, ti+1] = model['f'](x[:, ti], u[0, ti], 0)\n",
+    "y0 = model['h'](x, u)\n",
+    "y = y0 + (np.diag([0.6, 0.03])@np.random.normal(0, 1, size=(2, N)))\n",
+    "data = {'t': t, 'x': x, 'u': u, 'y0': y0, 'y': y}"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Plot nominal path"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 22,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "plt.figure(10, clear=True)\n",
+    "plt.plot(data['x'][0, :], data['x'][1, :])\n",
+    "plt.plot(data['x'][0, 0], data['x'][1, 0], 'bo')\n",
+    "plt.plot(data['x'][0, -1], data['x'][1, -1], 'rx')\n",
+    "plt.axis('square')\n",
+    "plt.xlabel('x')\n",
+    "plt.ylabel('y')\n",
+    "sns.despine()"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Plot measurements"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 23,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "plt.figure(20, clear=True)\n",
+    "plt.subplot(2, 1, 1)\n",
+    "plt.plot(data['t'], data['y0'][0], 'b', lw=2)\n",
+    "plt.plot(data['t'], data['y'][0], 'r')\n",
+    "plt.xlabel('t [s]')\n",
+    "plt.ylabel('[m]')\n",
+    "plt.title('Range measurement')\n",
+    "sns.despine()\n",
+    "\n",
+    "plt.subplot(2, 1, 2)\n",
+    "plt.plot(data['t'], data['y0'][1]*180/np.pi, 'b', lw=2)\n",
+    "plt.plot(data['t'], data['y'][1]*180/np.pi, 'r')\n",
+    "plt.xlabel('t [s]')\n",
+    "plt.ylabel('[deg]')\n",
+    "plt.title('Angle measurement')\n",
+    "sns.despine()\n",
+    "plt.tight_layout()"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Plot path computed from direct measurement"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Compute position estimate by inverting the measurement equation"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 28,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "dist = data['y'][0]\n",
+    "ang = data['y'][1]\n",
+    "pos_hat_0 = np.row_stack((dist*np.cos(ang), dist*np.sin(ang)))"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Plot the estimated path"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 31,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "plt.figure(30, clear=True)\n",
+    "plt.plot(data['x'][0], data['x'][1])\n",
+    "plt.plot(data['x'][0, 0], data['x'][1, 0], 'bo')\n",
+    "plt.plot(data['x'][0, -1], data['x'][1, -1], 'rx')\n",
+    "plt.plot(pos_hat_0[0], pos_hat_0[1], 'k')\n",
+    "plt.axis('square')\n",
+    "plt.xlabel('x')\n",
+    "plt.ylabel('y')\n",
+    "plt.title('Direct computation from measurements')\n",
+    "sns.despine()"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Plot position errors"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 32,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "plt.figure(31, clear=True)\n",
+    "plt.subplot(2, 1, 1)\n",
+    "plt.plot(data['t'], data['x'][0] - pos_hat_0[0])\n",
+    "plt.xlabel('t [s]');\n",
+    "plt.ylabel('[m]')\n",
+    "plt.title('x position error in direct computation');\n",
+    "sns.despine()\n",
+    "\n",
+    "plt.subplot(2, 1, 2)\n",
+    "plt.plot(data['t'], data['x'][1] - pos_hat_0[1])\n",
+    "plt.xlabel('t [s]');\n",
+    "plt.ylabel('[m]')\n",
+    "plt.title('y position error in direct computation')\n",
+    "sns.despine()\n",
+    "\n",
+    "plt.tight_layout()"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Extended Kalman Filter"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Implement your Extended Kalman Filter below"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 34,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "def EKF(model, init, data):\n",
+    "    \"\"\"Extended Kalman Filter\n",
+    "    \n",
+    "    Input:\n",
+    "      model - Dictionary with keys n, f, h, fx, hx, Q, R\n",
+    "      init - Dictionary with x0, P0\n",
+    "    \"\"\"\n",
+    "    M = data['y'].shape[1]  # Number of data points\n",
+    "    P = init['P0']  # Initial covariance P_0|-1\n",
+    "    n = model['n']  # Number of states\n",
+    "\n",
+    "    xhat = np.zeros((n, M))  # Prepare output array\n",
+    "    xhat[:, 0] = init['x0']  # Initialize xhat\n",
+    "\n",
+    "    for l in range(0, M - 1):\n",
+    "        pass\n",
+    "        # Measurement update\n",
+    "        # FILL IN YOUR CODE HERE\n",
+    "        \n",
+    "        # Time update\n",
+    "        # FILL IN YOUR CODE HERE\n",
+    "        \n",
+    "    return xhat"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Define model dictionary"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 35,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "modelEKF = {}\n",
+    "Q = None  # YOUR CODE HERE: 3x3 numpy array matrix, Process noise covariance \n",
+    "R = None # YOUR CODE HERE: 2x2 numpy array matrix, Measurement noise covariance\n",
+    "\n",
+    "modelEKF['n'] = 3\n",
+    "modelEKF['m'] = 2\n",
+    "modelEKF['Q'] = lambda x: Q\n",
+    "modelEKF['R'] = lambda x: R\n",
+    "modelEKF['f'] = lambda x, u: model['f'](x, u, 0)\n",
+    "modelEKF['h'] = lambda x, u: model['h'](x,u)\n",
+    "modelEKF['fx'] = lambda x, u: None  # YOUR CODE HERE: return df/dx(x, u)\n",
+    "modelEKF['hx'] = lambda x, u: None  # YOUR CODE HERE: return dh/dx(x, u)"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Define inital conditions and run filter"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "init = {}\n",
+    "P0 = None  # YOUR CODE HERE: 2x2 numpy array matrix, Initial estimate covariance\n",
+    "x0 = None  # YOUR CODE HERE: numpy array with initial state (x0, y0, theta0)\n",
+    "\n",
+    "init['x0'] = x0\n",
+    "init['P0'] = P0\n",
+    "\n",
+    "xhatEKF = EKF(modelEKF, init, data)"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Explore, investigate, and plot interesting results."
+   ]
+  },
+  {
+   "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.9.0"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 4
+}

Exercise/EKF_UKF/task1.m

+clear
+close all
+
+%% Simulate robot movement
+Ts = 0.1;
+V = 3;
+
+model.f = @(x,u,w) [x(1) + Ts*V*cos(x(3));x(2) + Ts*V*sin(x(3));x(3) + Ts*u];
+model.h = @(x,u) [sqrt(x(1,:).^2 + x(2,:).^2);atan(x(2,:)./x(1,:))];
+
+x0 = [10;10;0];
+Tfinal = 20;
+
+data.t = (0:Ts:Tfinal);
+N = numel(data.t);
+data.x = zeros(3,N); data.x(:,1) = x0;
+
+data.u = zeros(1,N);
+data.u(data.t>=5 & data.t < 10) = -1;
+data.u(data.t>=10 & data.t < 12) = 0;
+data.u(data.t>=12 & data.t < 15) = 1;
+data.u(data.t>=15 & data.t < 17) = -1;
+for t=1:N-1
+  data.x(:,t+1) = model.f(data.x(:,t),data.u(t),0);
+end
+data.y0 = model.h(data.x, data.u);
+data.y  = data.y0 + diag([0.6,0.03])*randn(2,N); 
+
+%% Plot mobile robot path
+figure(1)
+plot( data.x(1,:)', data.x(2,:)' )
+hold on
+plot( data.x(1,1)', data.x(2,1)', 'bo', ...
+  data.x(1,end)', data.x(2,end)', 'rx');
+hold off
+axis([0 max(data.x(1,:))*1.1 0 max(data.x(2,:))*1.1]);
+axis square
+xlabel('x');
+ylabel('y');
+
+%% Plot measurements
+figure(2)
+subplot( 211 )
+plot( data.t', [data.y(1,:)', data.y0(1,:)'])
+xlabel('t [s]');
+ylabel('[m]')
+title('Range measurement');
+box off
+
+subplot( 212 )
+plot( data.t', 180/pi*[data.y(2,:)', data.y0(2,:)'])
+xlabel('t [s]');
+ylabel('[deg]');
+title('Angle measurement');
+box off
+
+%% Plot path from direct measurements
+figure(3)
+plot( data.x(1,:)', data.x(2,:)' )
+hold on
+plot( data.x(1,1)', data.x(2,1)', 'bo', ...
+  data.x(1,end)', data.x(2,end)', 'rx');
+plot( (data.y(1,:).*cos(data.y(2,:)))', (data.y(1,:).*sin(data.y(2,:)))', 'k')
+hold off
+axis([0 max(data.x(1,:))*1.1 0 max(data.x(2,:))*1.1]);
+axis square
+xlabel('x');
+ylabel('y');
+title('Direct computation from measurements');
+
+%% EKF estimation
+modelEKF.f = @(x,u) FILL_IN_CODE_HERE;
+modelEKF.h = @(x,u) FILL_IN_CODE_HERE;
+modelEKF.fx = @(x,u) FILL_IN_CODE_HERE;
+modelEKF.hx = @(x,u) FILL_IN_CODE_HERE;
+modelEKF.Q = @(x) FILL_IN_CODE_HERE;
+modelEKF.R = @(x) FILL_IN_CODE_HERE;
+
+init.x0 = [10;5;0]; % Initial state estimate 
+init.P0 = FILL_IN_CODE_HERE; % Initial covariance
+tic; xhatEKF = EKF(modelEKF,init,data); toc
+
+%% Plot estimation results
+figure(20)
+plot( data.x(1,:)', data.x(2,:)', xhatEKF(1,:)', xhatEKF(2,:)' )
+hold on
+plot( data.x(1,1)', data.x(2,1)', 'bo', ...
+  data.x(1,end)', data.x(2,end)', 'rx');
+hold off
+axis([0 max(data.x(1,:))*1.1 0 max(data.x(2,:))*1.1]);
+axis square
+xlabel('x');
+ylabel('y');
+
+figure(21)
+subplot( 211 )
+plot( data.t', [xhatEKF(1,:)', data.x(1,:)'])
+xlabel('t [s]');
+ylabel('[m]')
+title('x position estimation');
+box off
+
+subplot( 212 )
+plot( data.t', xhatEKF(1,:)'-data.x(1,:)')
+xlabel('t [s]');
+ylabel('[m]')
+title('Estimation error');
+box off
+
+figure(22)
+subplot( 211 )
+plot( data.t', [xhatEKF(2,:)', data.x(2,:)'])
+xlabel('t [s]');
+ylabel('[m]')
+title('y position estimation');
+box off
+
+subplot( 212 )
+plot( data.t', xhatEKF(2,:)'-data.x(2,:)')
+xlabel('t [s]');
+ylabel('[m]');
+title('Estimation error');
+box off
+
+figure(23)
+subplot( 211 )
+plot( data.t', 180/pi*[xhatEKF(3,:)', data.x(3,:)'])
+xlabel('t [s]');
+ylabel('[deg]')
+title('Heading estimation');
+box off
+
+subplot( 212 )
+plot( data.t', 180/pi*(xhatEKF(3,:)'-data.x(3,:)'))
+xlabel('t [s]');
+ylabel('[deg]');
+title('Estimation error');
+box off

README.md

@@ -44,7 +44,7 @@ A preliminary course schedule is as follows. Extra discussion seminars can be ad
 | Discussion&nbsp;2 | March 26<br> 08:15 - 10:00 | Exercise discussion | [[30](https://doi.org/10.1016/j.automatica.2006.01.026)]||
 | Lecture 3 | April 9<br> 08:15 - 10:00 | Lec 3a: Design methods, EKF and UKF, square root algorithms |[[3](https://doi.org/10.1115/1.3153059), [4](https://doi.org/10.1080/00207178708933870)], [[17;p. 55-82](http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-11032)], [[22](https://www.fs.isy.liu.se/Edu/Courses/ObserversPhD/CourseMaterial/ArrayAlgorithm_Kailath.pdf), [9](https://doi.org/10.1109/TAC.1978.1101825), [14](https://doi.org/10.1109/JPROC.2003.823141), [23](https://doi.org/10.1109/TSP.2011.2172431)] | [slides](Lectures/slides_3.pdf)|
 | Lecture 4 | April 16<br> 08:15 - 10:00| Lec 3b: Design methods, Particle filters |||
-| Discussion&nbsp;3 | April 23<br> 08:15 - 10:00 | Exercise discussion|||
+| Discussion&nbsp;3 | April 23<br> 08:15 - 10:00 | Exercise discussion| [code skeletons](Exercise/EKF_UKF)||
 | Lecture 5 | April 30 <br> 08:15 - 10:00| Lec 4: Linearization, Lyapunov, High gain, Sliding mode, Moving Horizon Estimation |||
 | Discussion&nbsp;4 | TBD | Exercise discussion |||