{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"toc": "true"
},
"source": [
"# Table of Contents\n",
" "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Part (a)\n",
"\n",
"\n",
"\n",
"$$\\vec{x(t)} = \\begin{bmatrix} y(t) & \\dot{y(t)}\\end{bmatrix}^T$$\n",
"\n",
"\\begin{eqnarray*}\n",
"m\\ddot{y} + c\\dot{y} + ky = bu\\\\\n",
"\\ddot{y} + \\frac{c}{m}\\dot{y} + \\frac{k}{m}y = \\frac{b}{m}u\\\\\n",
"\\ddot{y} = -\\frac{c}{m}\\dot{y} - \\frac{k}{m}y \\frac{b}{m}u\\\\\n",
"\\end{eqnarray*}\n",
"\n",
"Thus we have:\n",
"\n",
"\\begin{eqnarray*}\n",
"\\frac{dy}{dt} = \\dot{y}\\\\\n",
"\\frac{d\\dot{y}}{dt} = -\\frac{c}{m}\\dot{y} - \\frac{k}{m}y + \\frac{b}{m}u\n",
"\\end{eqnarray*}\n",
"\n",
"Thus,\n",
"\\begin{eqnarray*}\n",
"\\frac{d}{dt}\\begin{bmatrix} y(t+1) \\\\ \\dot{y}(t+1) \\end{bmatrix} = \\begin{bmatrix} 0 & 1\\\\ -\\frac{k}{m} & -\\frac{c}{m} \\end{bmatrix} \\begin{bmatrix} y(t) \\\\ \\dot{y}(t) \\end{bmatrix} + \\begin{bmatrix} 0 \\\\ 1 \\end{bmatrix} u \\\\\n",
"\\frac{d}{dt}\\vec{x}(t+1) = \\begin{bmatrix} 0 & 1\\\\ -\\frac{k}{m} & -\\frac{c}{m} \\end{bmatrix} \\vec{x}(t) + \\begin{bmatrix} 0 \\\\ 1 \\end{bmatrix} u(t) \\\\\n",
"\\frac{d}{dt}\\vec{x}(t+1) = A \\vec{x}(t) + B u(t) \\\\\n",
"\\end{eqnarray*}\n",
"\n",
"and the equation for measurement is given by in the interval $[k\\tau, (k+1)\\tau)$:\n",
"\\begin{eqnarray*}\n",
"y_k = y(k\\tau) + \\omega_k\\\\\n",
"y_k = \\begin{bmatrix} 1 \\\\ 0\\end{bmatrix} \\begin{bmatrix} y(t) \\\\ \\dot{y}(t) \\end{bmatrix} + \\omega_k \\\\\n",
"y_k = \\begin{bmatrix} 1 \\\\ 0\\end{bmatrix} \\vec{x}_k + \\omega_k\n",
"\\end{eqnarray*}\n",
"\n",
"In the domain $[k\\tau, (k+1)\\tau)$ we refformulate the equations as:\n",
"\n",
"\\begin{eqnarray*}\n",
"\\frac{d}{dt}\\vec{x}(t+1) = A\\vec{x}(t) + Bu_t\\\\\n",
"\\text{where } A = \\begin{bmatrix} 0 & 1\\\\ -\\frac{k}{m} & -\\frac{c}{m}\\end{bmatrix}\\\\\n",
"\\text{and } B = \\begin{bmatrix} 0 \\\\ \\frac{b}{m} \\end{bmatrix} \n",
"\\end{eqnarray*}\n",
"\n",
"And the obseration equation:\n",
"\n",
"\\begin{eqnarray*}\n",
"y_k = C \\vec{x}_k + \\omega_k\\\\\n",
"\\text{where } C = \\begin{bmatrix} 1 \\\\ 0\\end{bmatrix} \n",
"\\end{eqnarray*}\n",
"\n",
"and the initial condition is given by:\n",
"\\begin{eqnarray*}\n",
"\\vec{x}(0) = \\begin{bmatrix} y(0) \\\\ \\dot{y}(0) \\end{bmatrix} = \\begin{bmatrix} y_0 + \\eta \\\\ y_1 + \\varsigma \\end{bmatrix} = \\begin{bmatrix} y_0 \\\\ y_1 \\end{bmatrix} + \\begin{bmatrix} \\eta \\\\ \\varsigma \\end{bmatrix} = \\begin{bmatrix} y_0 \\\\ y_1 \\end{bmatrix} + \\vec{\\epsilon} \\\\\n",
"\\text{where } \\vec{\\epsilon} = \\mathcal{N}(\\begin{bmatrix} 0 \\\\ 0 \\end{bmatrix}, Q)\\\\\n",
"\\text{and } Q = \\begin{bmatrix} \\alpha^2 & 0 \\\\ 0 & \\beta^2 \\end{bmatrix} \\\\\n",
"\\text{where } \\eta \\sim \\mathcal{N}(0, \\alpha^2) \\\\\n",
"\\text{and } \\varsigma \\sim \\mathcal{N}(0, \\beta^2)\n",
"\\end{eqnarray*}"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We now use variation of parameters on the interval $[k\\tau, (k+1)\\tau)$ t obtain:\n",
"\n",
"\\begin{eqnarray*}\n",
"\\vec{x}_{k+1} = \\exp(A\\tau)x_{k} + \\int_{k\\tau}^{k\\tau+\\tau} \\exp(A(s-k\\tau)) B(s) u(s) ds\\\\\n",
"\\vec{x}_{k+1} = \\exp(A\\tau)x_{k} + \\int_{k\\tau}^{k\\tau+\\tau} \\exp(A(s-k\\tau)) ds B(u_k + \\epsilon_k) \\\\\n",
"\\vec{x}_{k+1} = \\exp(A\\tau)x_{k} + \\int_{0}^{\\tau} \\exp(As') ds' B(u_k + \\epsilon_k) \\ \\text{where } s'=s+k\\tau \\\\\n",
"\\vec{x}_{k+1} = \\exp(A\\tau)x_{k} + \\int_{0}^{\\tau} \\exp(As') ds' B(u_k + \\epsilon_k) \\\\\n",
"\\vec{x}_{k+1} = \\exp(A\\tau)x_{k} + A^{-1}(\\exp(A\\tau)-I)Bu_k + A^{-1}(\\exp(A\\tau)-I)B\\epsilon_k \\\\\n",
"\\end{eqnarray*}\n",
"\n",
"Now, \n",
"\n",
"\\begin{eqnarray*}\n",
"\\exp(A\\tau) = I + \\frac{A\\tau}{1!} + \\frac{A^2\\tau^2}{2!} + \\frac{A^3\\tau^3}{3!} + \\dots \n",
"\\end{eqnarray*}\n",
"\n",
"\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Part (b)\n",
"\\begin{eqnarray*}\n",
"\\vec{x}_{k+1} = \\phi \\vec{x}_k + \\psi u_k + \\vec{v}_k\\\\\n",
"\\end{eqnarray*}\n",
"\n",
"The cofficients of the above equation are given by:\n",
"\n",
"\\begin{eqnarray*}\n",
"A = \\begin{bmatrix} 0 & 1\\\\ -\\frac{k}{m} & -\\frac{c}{m}\\end{bmatrix}\\\\\n",
"\\text{and } B = \\begin{bmatrix} 0 \\\\ \\frac{b}{m} \\end{bmatrix} \\\\\n",
"phi = \\exp(A\\tau)\\\\\n",
"\\psi = A^{-1}(\\exp(A\\tau)-I)B\\\\\n",
"\\vec{v}_k = A^{-1}(\\exp(A\\tau)-I)B\\epsilon_k = \\psi \\epsilon_k\n",
"\\end{eqnarray*}\n",
"\n",
"And thus $\\vec{v}_k \\sim \\mathcal{N}(\\begin{bmatrix} 0 \\\\ 0 \\end{bmatrix}, V)$ where $V = \\sigma^2 \\psi \\psi^T$ where $\\psi_{2 \\times 1} = A^{-1}(\\exp(A\\tau)-I)B$\n"
]
},
{
"cell_type": "markdown",
"metadata": {
"collapsed": true
},
"source": [
"# Part (c)\n",
"\n",
"\n",
"\\begin{eqnarray*}\n",
"x_{k+1} = \\phi x_k + \\psi u_k + v_k\\\\\n",
"\\bar{x}_{k+1} = \\phi \\bar{x_k} + \\psi u_k \\\\\n",
"x_{k+1} - \\bar{x}_{k+1} = \\phi (x_k-\\bar{x_k}) + v_k\\\\\n",
"\\end{eqnarray*}\n",
"\n",
"\\begin{align*}\n",
"P_{k+1|k} &= E((x_{k+1} - \\bar{x}_{k+1})(x_{k+1} - \\bar{x}_{k+1})^T)\\\\\n",
"&= E[(\\phi (x_k-\\bar{x_k}) + v_k)((x_k-\\bar{x_k})^T\\phi^T + v_k^T)]\\\\\n",
"&= E[\\phi(x_k-\\bar{x_k})(x_k-\\bar{x_k})^T\\phi^T + 2(x_k-\\bar{x_k})\\phi v_k + v_k v_k^T]\\\\\n",
"&= \\phi P_{k|k} \\phi^T + V_k\n",
"\\end{align*}"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Prediction\n",
"\n",
"\\begin{align*}\n",
"\\hat{x}_{k+1|k} &= \\phi \\hat{x}_k + \\psi u_k\\\\\n",
"P_{k+1|k} &= \\phi P_{k|k} \\phi^T + V_k\n",
"\\end{align*}"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Observed/Measured\n",
"\n",
"\\begin{align*}\n",
"x_{k+1|k} &= \\phi x_k + \\psi u_k + \\vec{v}_k\\\\\n",
"y_{k+1|k} &= Cx_{k+1} + \\omega_k \n",
"\\end{align*}"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Update\n",
"\n",
"Assume update step to be linear combination of prediction and observation. We need to obtain $K_1, K_2$ such that the estimator is unbiased and is minimum variance estimate.\n",
"\n",
"\\begin{align*}\n",
"\\hat{x}_{k+1|k+1} &= K_1 \\hat{x}_{k+1|k} + K_2 y_{k+1|k}\n",
"\\end{align*}\n",
"\n",
"Define error $e_{k+1|k+1} = \\hat{x}_{k+1|k+1} - x_{k+1}$\n",
"\n",
"We look for unbiased estimator $E[e_{k+1|k+1}]=0$\n",
"\n",
"Thus,\n",
"\n",
"\\begin{align*}\n",
"e_{k+1|k+1} &= \\hat{x}_{k+1|k+1} - x_{k+1}\\\\\n",
"&= K_1 \\hat{x}_{k+1|k} +K_2(Cx_{k+1}+\\omega_{k+1}) - x_{k+1}\\\\\n",
"&= K_1 \\hat{x}_{k+1|k} +(K_2C-I)x_{k+1} + K_2\\omega_{k+1}\\\\\n",
"\\end{align*}\n",
"\n",
"Now, \n",
"\n",
"\\begin{align*}\n",
"E[e_{k+1|k+1}] &=0\\\\\n",
"\\implies K_1 &= I-K_2C\n",
"\\end{align*}\n",
"\n",
"\n",
"Thus, \n",
"\\begin{align*}\n",
"\\hat{x}_{k+1|k+1} &= (I-K_2C) \\hat{x}_{k+1|k} + K_2 y_{k+1|k}\\\\\n",
"&= (I-K_2C) \\hat{x}_{k+1|k} + K_2(Cx_{k+1}+\\omega_{k+1})\\\\\n",
"&= \\hat{x}_{k+1|k} + K_2C(x_{k+1}-\\hat{x}_{k+1|k}) + \\omega_{k+1}\n",
"\\end{align*}\n",
"\n",
"Now, \n",
"\\begin{align*}\n",
"e_{k+1|k+1} &= \\hat{x}_{k+1|k+1} - x_{k+1}\\\\\n",
"&= K_1 \\hat{x}_{k+1|k} +K_2(Cx_{k+1}+\\omega_{k+1}) - x_{k+1}\\\\\n",
"&= (I-K_2C)\\hat{x}_{k+1|k} +(K_2C-I)x_{k+1} + K_2\\omega_{k+1}\\\\\n",
"&= (I-K_2C)(\\hat{x}_{k+1|k}-x_{k+1}) + K_2\\omega_{k+1}\\\\\n",
"&= (I-K_2C)e_{k+1|k} + K_2\\omega_{k+1}\\\\\n",
"\\end{align*}\n",
"\n",
"where $e_{k+1|k} = \\hat{x}_{k+1|k}-x_{k+1}$\n",
"Then,\n",
"\\begin{align*}\n",
"P_{k+1|k+1} &= E[e_{k+1|k+1}e_{k+1|k+1}^T] \\\\\n",
"&= E[((I-K_2C)e_{k+1|k} + K_2\\omega_{k+1}) ((I-K_2C)e_{k+1|k} + K_2\\omega_{k+1})^T ]\\\\\n",
"&= (I-K_2C)E[e_{k+1|k}^Te_{k+1|k}] (I-K_2C)^T + K_2E[\\omega_{k+1}\\omega_{k+1}^T]K_2^T\\\\\n",
"&= (I-K_2C)P_{k+1|k}(I-K_2C)^T + K_2W_kK_2^T\\\\\n",
"&= (P_{k+1|k} - K_2CP_{k+1|k}) (I-K_2C)^T+K_2W_kK_2^T\\\\\n",
"&= P_{k+1|k} - K_2CP_{k+1|k} - P_{k+1|k}C^TK_2^T + K_2CP_{k+1|k}C^TK_2^T + K_2W_kK_2^T\\\\\n",
"&= P_{k+1|k} - K_2CP_{k+1|k} - P_{k+1|k}C^TK_2^T + K_2( CP_{k+1|k}C^T + W_k)K_2^T\\\\\n",
"\\end{align*}\n",
"\n",
"To minimize $\\frac{\\partial P_{k+1|k+1}}{\\partial K_2} =0$\n",
"\n",
"\\begin{align*}\n",
"\\frac{\\partial P_{k+1|k+1}}{\\partial K_2} = -2 P_{k+1|k}^TC + 2K_2(CP_{k+1|k}C^T + W_k) = 0\\\\\n",
"\\implies K_2 = P_{k+1|k}^TC(CP_{k+1|k}C^T + W_k)^{-1}\n",
"\\end{align*}\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Prediction\n",
"\n",
"\\begin{align*}\n",
"\\hat{x}_{k+1|k} &= \\phi \\hat{x}_k + \\psi u_k\\\\\n",
"P_{k+1|k} &= \\phi P_{k|k} \\phi^T + V_k\n",
"\\end{align*}\n",
"\n",
"### Observed/Measured\n",
"\n",
"\\begin{align*}\n",
"x_{k+1|k} &= \\phi x_k + \\psi u_k + \\vec{v}_k\\\\\n",
"y_{k+1|k} &= Cx_{k+1} + \\omega_k \n",
"\\end{align*}\n",
"\n",
"### Update Step\n",
"\\begin{align*}\n",
"\\hat{x}_{k+1|k+1} &= \\hat{x}_{k+1|k} + K_2(y_k - C\\hat{x}_{k+1|k})\\\\\n",
"P_{k+1|k+1} &= P_{k+1|k} - K_2CP_{k+1|k} \\\\\n",
"K_2 &= P_{k+1|k}^TC(CP_{k+1|k}C^T + W_k)^{-1}\n",
"\\end{align*}"
]
}
],
"metadata": {
"hide_input": false,
"kernelspec": {
"display_name": "Python [default]",
"language": "python",
"name": "python2"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 2
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython2",
"version": "2.7.11"
},
"latex_envs": {
"LaTeX_envs_menu_present": true,
"autocomplete": true,
"bibliofile": "biblio.bib",
"cite_by": "apalike",
"current_citInitial": 1,
"eqLabelWithNumbers": true,
"eqNumInitial": 1,
"hotkeys": {
"equation": "Ctrl-E",
"itemize": "Ctrl-I"
},
"labels_anchors": false,
"latex_user_defs": false,
"report_style_numbering": false,
"user_envs_cfg": false
},
"toc": {
"colors": {
"hover_highlight": "#DAA520",
"navigate_num": "#000000",
"navigate_text": "#333333",
"running_highlight": "#FF0000",
"selected_highlight": "#FFD700",
"sidebar_border": "#EEEEEE",
"wrapper_background": "#FFFFFF"
},
"moveMenuLeft": true,
"nav_menu": {
"height": "178px",
"width": "252px"
},
"navigate_menu": true,
"number_sections": true,
"sideBar": true,
"threshold": 4,
"toc_cell": true,
"toc_section_display": "block",
"toc_window_display": true,
"widenNotebook": false
}
},
"nbformat": 4,
"nbformat_minor": 1
}