diff --git a/Exercise/EKF_UKF/EKF.m b/Exercise/EKF_UKF/EKF.m
new file mode 100644
index 0000000000000000000000000000000000000000..588df4f60958f11d55f61c01d55cfd8e4948763a
--- /dev/null
+++ b/Exercise/EKF_UKF/EKF.m
@@ -0,0 +1,16 @@
+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
diff --git a/Exercise/EKF_UKF/task1.ipynb b/Exercise/EKF_UKF/task1.ipynb
new file mode 100644
index 0000000000000000000000000000000000000000..7ef82a7e1640471147f702d7756aa84821871747
--- /dev/null
+++ b/Exercise/EKF_UKF/task1.ipynb
@@ -0,0 +1,360 @@
+{
+ "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
+}
diff --git a/Exercise/EKF_UKF/task1.m b/Exercise/EKF_UKF/task1.m
new file mode 100644
index 0000000000000000000000000000000000000000..2743e3bd5709b5d8a3c2e376715fdbd00a5a13b0
--- /dev/null
+++ b/Exercise/EKF_UKF/task1.m
@@ -0,0 +1,138 @@
+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
diff --git a/Lectures/slides_3.pdf b/Lectures/slides_3.pdf
index 7b37e671c899216da79dca63a5de5537c22b60f1..782e67468a3b05912fd59b48489056f27f0975ea 100644
Binary files a/Lectures/slides_3.pdf and b/Lectures/slides_3.pdf differ
diff --git a/README.md b/README.md
index 71463751e174c0c12b052fa21d10988402c49d1b..cdeb21afabad8293c0354cf2ddc8616a66b41ec9 100644
--- a/README.md
+++ b/README.md
@@ -44,7 +44,7 @@ A preliminary course schedule is as follows. Extra discussion seminars can be ad
| Discussion 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 3 | April 23<br> 08:15 - 10:00 | Exercise discussion|||
+| Discussion 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 4 | TBD | Exercise discussion |||