Lecture 19

.gitignore

@@ -28,3 +28,5 @@ Lecture 15/.ipynb_checkpoints/
 Lecture 16/.ipynb_checkpoints/
 
 Lecture 17/.ipynb_checkpoints/
+
+Lecture 19/.ipynb_checkpoints/

Lecture 19/Manifest.toml

@@ -0,0 +1,2 @@
+# This file is machine-generated - editing it directly is not advised
+

Lecture 19/lqg.ipynb

@@ -0,0 +1,263 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "f8a3bd70-079d-42e5-925f-0328a8657725",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "bc71ef22-6de2-4f40-98ce-3d933dfc7106",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "using LinearAlgebra\n",
+    "using PyPlot\n",
+    "using SparseArrays\n",
+    "using ControlSystems\n",
+    "using ForwardDiff"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "7e13e806-4bac-4fa0-ab63-679aaa53643b",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Discrete dynamics\n",
+    "h = 0.1   # time step\n",
+    "A = [1 h; 0 1]\n",
+    "B = [0.5*h*h; h]\n",
+    "C = [1.0 0]"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "5e3da132-9212-49a2-9d85-8199beae0eab",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Noise Covariances\n",
+    "W = B*0.1*B' #Corresponds to white noise force input to dynamics\n",
+    "V = 0.1 #Noise on position measurements"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "87a50558-c7a0-435e-83ab-c982ceba8bf9",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "n = 2     # number of state\n",
+    "m = 1     # number of controls\n",
+    "Tfinal = 10.0 # final time\n",
+    "N = Int(Tfinal/h)+1    # number of time steps\n",
+    "thist = Array(range(0,h*(N-1), step=h));"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "55ec4049-4cdb-47ca-b37d-9e402f18ad34",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# LQR Cost weights\n",
+    "Q = Array(1.0*I(n))\n",
+    "R = Array(0.1*I(m))\n",
+    "Qn = Q"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "41cbec2b-233b-47ad-9ff1-f16bc7a4207c",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "#Cost function\n",
+    "function J(xhist,uhist)\n",
+    "    cost = 0.5*xhist[:,end]'*Qn*xhist[:,end]\n",
+    "    for k = 1:(size(xhist,2)-1)\n",
+    "        cost = cost + 0.5*xhist[:,k]'*Q*xhist[:,k] + 0.5*(uhist[k]'*R*uhist[k])[1]\n",
+    "    end\n",
+    "    return cost\n",
+    "end"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "05bafaba-dd76-4f86-8a16-b9cb07f0cb97",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "#Infinite-Horizon LQR Gain and cost-to-go\n",
+    "P = dare(A,B,Q,R)\n",
+    "K = dlqr(A,B,Q,R)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "0ce5e70f-c45d-486b-9a99-3cd4c534132f",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Initial conditions\n",
+    "x0 = [1.0; 0] #True state\n",
+    "\n",
+    "#Filter state\n",
+    "Σ0 = Array(1.0*I(2))\n",
+    "x̂0 = [0.0; 0.0]"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "3a91498c-c9f3-44ee-b183-b332fe6c8ffc",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "xhist = zeros(n,N)\n",
+    "xhist[:,1] .= x0;\n",
+    "\n",
+    "uhist = zeros(N)\n",
+    "yhist = zeros(N)\n",
+    "x̂hist = zeros(n,N)\n",
+    "Σhist = zeros(n,n,N);"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "dbd85ccc-42e1-4b60-86e8-0797301744a5",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "#Initial Time Step\n",
+    "\n",
+    "#Generate Measurement\n",
+    "yhist[1] = (C*xhist[:,1])[1] + sqrt(V)*randn()\n",
+    "\n",
+    "z = yhist[1] - (C*x̂0)[1] #Innovation\n",
+    "S = (C*Σ0*C')[1] + V #Innovation Covariance\n",
+    "\n",
+    "L = Σ0*C'*inv(S) #Kalman Gain\n",
+    "\n",
+    "x̂hist[:,1] = x̂0 + L*z\n",
+    "Σhist[:,:,1] .= (I-L*C)*Σ0*(I-L*C)' + L*V*L'\n",
+    "\n",
+    "uhist[1] = -(K*x̂hist[:,1])[1] #Control input\n",
+    "\n",
+    "xhist[:,2] .= A*xhist[:,1] + B*uhist[1] + sqrt(W)*randn(n) #Simulate with stochastic dynamics"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "6dae90a3-948a-40de-9708-cd5f51c1bd46",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "for k = 2:(N-1)\n",
+    "    \n",
+    "    #Generate measurement\n",
+    "    yhist[k] = (C*xhist[:,k])[1] + sqrt(V)*randn()\n",
+    "\n",
+    "    #KF Update\n",
+    "    x̃ = A*x̂hist[:,k-1] + B*uhist[k-1] #State Prediction\n",
+    "    Σ̃ = A*Σhist[:,:,k-1]*A' + W #Covariance Prediction\n",
+    "\n",
+    "    z = yhist[k] - (C*x̃)[1] #Innovation\n",
+    "    S = (C*Σ̃*C')[1] + V #Innovation Covariance\n",
+    "\n",
+    "    L = Σ̃*C'*inv(S) #Kalman Gain\n",
+    "\n",
+    "    x̂hist[:,k] = x̃ + L*z\n",
+    "    Σhist[:,:,k] = (I-L*C)*Σ̃*(I-L*C)' + L*V*L'\n",
+    "\n",
+    "    #LQR Controller\n",
+    "    uhist[k] = -(K*x̂hist[:,k])[1]\n",
+    "    \n",
+    "    #Run this on the stochastic dynamics\n",
+    "    xhist[:,k+1] .= A*xhist[:,k] + B*uhist[k] + sqrt(W)*randn(n)\n",
+    "end"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "b80c5056-0361-4be0-9cc8-f89094aecbb6",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "plot(xhist[1,:])"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "90ea7b22-87e8-4775-be5f-dac456e59399",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "plot(x̂hist[1,:])"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "6fe7606d-3ba8-48a0-84c7-26890e2d393e",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "#Covariance and Kalman gain converge to steady-state values in the LTI case\n",
+    "#(just like LQR gain + cost-to-go Hessian)\n",
+    "plot(Σhist[1,1,1:N-1])\n",
+    "plot(Σhist[2,2,1:N-1])\n",
+    "plot(Σhist[1,2,1:N-1])"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "071f1ac5-b1d0-43d2-9739-5cd790f9a42c",
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "dd7d1415-a5f6-40b7-8211-0977222630c3",
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Julia 1.6.5",
+   "language": "julia",
+   "name": "julia-1.6"
+  },
+  "language_info": {
+   "file_extension": ".jl",
+   "mimetype": "application/julia",
+   "name": "julia",
+   "version": "1.6.5"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}