Layout changes to make PDF read better.
Mostly just shortening lines of code. There was a SymPy font render issue as well.
This commit is contained in:
Roger Labbe
parent
c1281bc47c
commit
3367806b07
File diff suppressed because one or more lines are too long
@ -602,8 +602,8 @@
|
||||
"input": [
|
||||
"import numpy\n",
|
||||
"def perfect_predict(pos, move):\n",
|
||||
" \"\"\" move the position by 'move' spaces, where positive is to the right, and negative\n",
|
||||
" is to the left\n",
|
||||
" \"\"\" move the position by 'move' spaces, where positive is to the right, \n",
|
||||
" and negative is to the left\n",
|
||||
" \"\"\"\n",
|
||||
" n = len(pos)\n",
|
||||
" result = np.array(pos, dtype=float)\n",
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
{
|
||||
"metadata": {
|
||||
"name": "",
|
||||
"signature": "sha256:00a931decd323bc4f252540ee5dd36a369b885268e890c562b4a210ea2fff34b"
|
||||
"signature": "sha256:71e5314e7d6889b41a67a2adfc7f9a248d967c2d98a94f399a428d8a11833da2"
|
||||
},
|
||||
"nbformat": 3,
|
||||
"nbformat_minor": 0,
|
||||
@ -607,7 +607,8 @@
|
||||
" plt.ylim((0,1))\n",
|
||||
" plt.show()\n",
|
||||
"\n",
|
||||
"interact (plt_g, mu=(0,10), variance=widgets.FloatSliderWidget(value=0.6,min=0.2,max=4.5))"
|
||||
"interact (plt_g, mu=(0,10), \n",
|
||||
" variance=widgets.FloatSliderWidget(value=0.6,min=0.2,max=4.5))"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
@ -1,7 +1,7 @@
|
||||
{
|
||||
"metadata": {
|
||||
"name": "",
|
||||
"signature": "sha256:a223b7e436fc202bc6f77b7834cdaa6338449361293f2c25146fc75469ca4718"
|
||||
"signature": "sha256:26a72ead7479bc0a3d34d1516bcea91514ebd0335f33730b27a5299b7d7a8f0b"
|
||||
},
|
||||
"nbformat": 3,
|
||||
"nbformat_minor": 0,
|
||||
@ -726,9 +726,7 @@
|
||||
"\n",
|
||||
"$$\\mathbf{Q} = \\int_0^{\\Delta t} \\Phi(t)\\mathbf{Q_c}\\Phi^T(t) dt$$\n",
|
||||
"\n",
|
||||
"where $\\mathbf{Q_c}$ is the continuous noise.\n",
|
||||
"\n",
|
||||
"This gives us\n",
|
||||
"where $\\mathbf{Q_c}$ is the continuous noise. This gives us\n",
|
||||
"\n",
|
||||
"$$\\Phi = \\begin{bmatrix}1 & \\Delta t & {\\Delta t}^2/2 \\\\ 0 & 1 & \\Delta t\\\\ 0& 0& 1\\end{bmatrix}$$\n",
|
||||
"\n",
|
||||
|
||||
File diff suppressed because one or more lines are too long
@ -259,13 +259,13 @@
|
||||
],
|
||||
"metadata": {},
|
||||
"output_type": "pyout",
|
||||
"prompt_number": 1,
|
||||
"prompt_number": 2,
|
||||
"text": [
|
||||
"<IPython.core.display.HTML at 0x7f4ddac347b8>"
|
||||
"<IPython.core.display.HTML at 0x7f7bfc0eda20>"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 1
|
||||
"prompt_number": 2
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -547,8 +547,10 @@
|
||||
"for i in range(10):\n",
|
||||
" out = h(out)\n",
|
||||
" out2 = g(out2)\n",
|
||||
"print ('linear output mean, variance: %.4f, %.4f'% (np.average(out), np.std(out)**2))\n",
|
||||
"print ('nonlinear output mean, variance: %.4f, %.4f'% (np.average(out2), np.std(out2)**2))"
|
||||
"print ('linear output mean, variance: %.4f, %.4f' % \n",
|
||||
" (np.average(out), np.std(out)**2))\n",
|
||||
"print ('nonlinear output mean, variance: %.4f, %.4f' % \n",
|
||||
" (np.average(out2), np.std(out2)**2))"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
@ -815,7 +817,7 @@
|
||||
"\n",
|
||||
"So we want to track the position of an aircraft assuming a constant velocity and altitude, and measurements of the slant distance to the aircraft. That means we need 3 state variables - horizontal distance, velocity, and altitude.\n",
|
||||
"\n",
|
||||
"$$ $\\mathbf{x} = \\begin{bmatrix}distance \\\\velocity\\\\ altitude\\end{bmatrix}= \\begin{bmatrix}x_{pos} \\\\x_{vel}\\\\ x_{alt}\\end{bmatrix}$$"
|
||||
"$$\\mathbf{x} = \\begin{bmatrix}distance \\\\velocity\\\\ altitude\\end{bmatrix}= \\begin{bmatrix}x_{pos} \\\\x_{vel}\\\\ x_{alt}\\end{bmatrix}$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
@ -973,7 +975,10 @@
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"def hx(x):\n",
|
||||
" \"\"\" compute measurement for slant range that would correspond to state x\"\"\"\n",
|
||||
" \"\"\" compute measurement for slant range that would correspond \n",
|
||||
" to state x.\n",
|
||||
" \"\"\"\n",
|
||||
" \n",
|
||||
" return (x[0,0]**2 + x[2,0]**2) ** 0.5"
|
||||
],
|
||||
"language": "python",
|
||||
@ -1150,13 +1155,16 @@
|
||||
"\n",
|
||||
"Depending on your experience with derivatives you may have found the computation of the Jacobian above either fairly straightforward, or quite difficult. Even if you found it easy, a slightly more difficult problem easily leads to very difficult computations.\n",
|
||||
"\n",
|
||||
"As explained in Appendix C, we can use the SymPy package to compute the Jacobian for us. "
|
||||
"As explained in Appendix A, we can use the SymPy package to compute the Jacobian for us. "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"import sympy\n",
|
||||
"sympy.init_printing(use_latex='mathjax')\n",
|
||||
"\n",
|
||||
"x_pos, x_vel, x_alt = sympy.symbols('x_pos, x_vel x_alt')\n",
|
||||
"\n",
|
||||
"H = sympy.Matrix([sympy.sqrt(x_pos**2 + x_alt**2)])\n",
|
||||
@ -1173,7 +1181,7 @@
|
||||
],
|
||||
"metadata": {},
|
||||
"output_type": "pyout",
|
||||
"prompt_number": 48,
|
||||
"prompt_number": 4,
|
||||
"text": [
|
||||
"\u23a1 x_pos x_alt \u23a4\n",
|
||||
"\u23a2\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 0 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u23a5\n",
|
||||
@ -1183,7 +1191,7 @@
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 48
|
||||
"prompt_number": 4
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -1229,6 +1237,7 @@
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"import sympy\n",
|
||||
"sympy.init_printing(use_latex='mathjax')\n",
|
||||
"w_vel, w_alt, dt = sympy.symbols('w_vel w_alt \\Delta{t}')\n",
|
||||
"w = sympy.Matrix([[0, w_vel, w_alt]]).T\n",
|
||||
"phi = sympy.Matrix([[1, dt, 0], [0, 1, 0], [0,0,1]])\n",
|
||||
@ -1246,7 +1255,7 @@
|
||||
],
|
||||
"metadata": {},
|
||||
"output_type": "pyout",
|
||||
"prompt_number": 15,
|
||||
"prompt_number": 5,
|
||||
"text": [
|
||||
"\u23a1 3 2 2 2 2 \u23a4\n",
|
||||
"\u23a2 \\Delta{t} \u22c5w_vel \\Delta{t} \u22c5w_vel \\Delta{t} \u22c5w_alt\u22c5w_vel\u23a5\n",
|
||||
@ -1265,156 +1274,7 @@
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 15
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"f21, f22, t, phi = sympy.symbols('f_21 f_22 t \\Phi_S')\n",
|
||||
"Q_t = sympy.Matrix([[0,0],[0,phi]])\n",
|
||||
"f = sympy.Matrix([[1,dt],[f21, 1+f22*dt]])\n",
|
||||
"term = f*Q_t*f.T\n",
|
||||
"term\n",
|
||||
"sympy.integrate(term,(dt, 0, dt))\n"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"latex": [
|
||||
"$$\\left[\\begin{matrix}\\frac{\\Delta{t}^{3} \\Phi_{S}}{3} & \\frac{\\Phi_{S} f_{22}}{3} \\Delta{t}^{3} + \\frac{\\Delta{t}^{2} \\Phi_{S}}{2}\\\\\\frac{\\Phi_{S} f_{22}}{3} \\Delta{t}^{3} + \\frac{\\Delta{t}^{2} \\Phi_{S}}{2} & \\frac{\\Delta{t}^{3} \\Phi_{S}}{3} f_{22}^{2} + \\Delta{t}^{2} \\Phi_{S} f_{22} + \\Delta{t} \\Phi_{S}\\end{matrix}\\right]$$"
|
||||
],
|
||||
"metadata": {},
|
||||
"output_type": "pyout",
|
||||
"prompt_number": 40,
|
||||
"text": [
|
||||
"\u23a1 3 3 \n",
|
||||
"\u23a2 \\Delta{t} \u22c5\\Phi_S \\Delta{t} \u22c5\\Phi_S\u22c5f\u2082\u2082 \n",
|
||||
"\u23a2 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \n",
|
||||
"\u23a2 3 3 \n",
|
||||
"\u23a2 \n",
|
||||
"\u23a2 3 2 3 2 \n",
|
||||
"\u23a2\\Delta{t} \u22c5\\Phi_S\u22c5f\u2082\u2082 \\Delta{t} \u22c5\\Phi_S \\Delta{t} \u22c5\\Phi_S\u22c5f\u2082\u2082 \n",
|
||||
"\u23a2\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 + \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 + \\Delta{t}\n",
|
||||
"\u23a3 3 2 3 \n",
|
||||
"\n",
|
||||
" 2 \u23a4\n",
|
||||
" \\Delta{t} \u22c5\\Phi_S \u23a5\n",
|
||||
"+ \u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500 \u23a5\n",
|
||||
" 2 \u23a5\n",
|
||||
" \u23a5\n",
|
||||
" \u23a5\n",
|
||||
"2 \u23a5\n",
|
||||
" \u22c5\\Phi_S\u22c5f\u2082\u2082 + \\Delta{t}\u22c5\\Phi_S\u23a5\n",
|
||||
" \u23a6"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 40
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"#Bar-Shalom P270\n",
|
||||
"T,t,dt = sympy.symbols('T t \\Delta{t}')\n",
|
||||
"v_k = sympy.Matrix([[T-t],[1]])\n",
|
||||
"term = v_k*v_k.T\n",
|
||||
"term\n",
|
||||
"sympy.integrate(v_k*v_k.T,(t, 0, T))\n",
|
||||
"\n"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"latex": [
|
||||
"$$\\left[\\begin{matrix}\\frac{T^{3}}{3} & \\frac{T^{2}}{2}\\\\\\frac{T^{2}}{2} & T\\end{matrix}\\right]$$"
|
||||
],
|
||||
"metadata": {},
|
||||
"output_type": "pyout",
|
||||
"prompt_number": 51,
|
||||
"text": [
|
||||
"\u23a1 3 2\u23a4\n",
|
||||
"\u23a2T T \u23a5\n",
|
||||
"\u23a2\u2500\u2500 \u2500\u2500\u23a5\n",
|
||||
"\u23a23 2 \u23a5\n",
|
||||
"\u23a2 \u23a5\n",
|
||||
"\u23a2 2 \u23a5\n",
|
||||
"\u23a2T \u23a5\n",
|
||||
"\u23a2\u2500\u2500 T \u23a5\n",
|
||||
"\u23a32 \u23a6"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 51
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"#Bar-Shalom P272\n",
|
||||
"T,t,dt = sympy.symbols('T t \\Delta{t}')\n",
|
||||
"v_k = sympy.Matrix([[(T-t)*(T-t)/2], [T-t], [1]])\n",
|
||||
"term = v_k*v_k.T\n",
|
||||
"term\n",
|
||||
"print(sympy.pretty(sympy.integrate(v_k*v_k.T,(t, 0, T))))\n",
|
||||
"((T-t)*(T-t)/2).expand()\n",
|
||||
"\n",
|
||||
"wk=sympy.Matrix([[T**2/2,T,1]]).T\n",
|
||||
"wk*wk.T\n",
|
||||
"sympy.integrate(wk*wk.T, T)"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"output_type": "stream",
|
||||
"stream": "stdout",
|
||||
"text": [
|
||||
"\u23a1 5 4 3\u23a4\n",
|
||||
"\u23a2T T T \u23a5\n",
|
||||
"\u23a2\u2500\u2500 \u2500\u2500 \u2500\u2500\u23a5\n",
|
||||
"\u23a220 8 6 \u23a5\n",
|
||||
"\u23a2 \u23a5\n",
|
||||
"\u23a2 4 3 2\u23a5\n",
|
||||
"\u23a2T T T \u23a5\n",
|
||||
"\u23a2\u2500\u2500 \u2500\u2500 \u2500\u2500\u23a5\n",
|
||||
"\u23a28 3 2 \u23a5\n",
|
||||
"\u23a2 \u23a5\n",
|
||||
"\u23a2 3 2 \u23a5\n",
|
||||
"\u23a2T T \u23a5\n",
|
||||
"\u23a2\u2500\u2500 \u2500\u2500 T \u23a5\n",
|
||||
"\u23a36 2 \u23a6\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"latex": [
|
||||
"$$\\left[\\begin{matrix}\\frac{T^{5}}{20} & \\frac{T^{4}}{8} & \\frac{T^{3}}{6}\\\\\\frac{T^{4}}{8} & \\frac{T^{3}}{3} & \\frac{T^{2}}{2}\\\\\\frac{T^{3}}{6} & \\frac{T^{2}}{2} & T\\end{matrix}\\right]$$"
|
||||
],
|
||||
"metadata": {},
|
||||
"output_type": "pyout",
|
||||
"prompt_number": 72,
|
||||
"text": [
|
||||
"\u23a1 5 4 3\u23a4\n",
|
||||
"\u23a2T T T \u23a5\n",
|
||||
"\u23a2\u2500\u2500 \u2500\u2500 \u2500\u2500\u23a5\n",
|
||||
"\u23a220 8 6 \u23a5\n",
|
||||
"\u23a2 \u23a5\n",
|
||||
"\u23a2 4 3 2\u23a5\n",
|
||||
"\u23a2T T T \u23a5\n",
|
||||
"\u23a2\u2500\u2500 \u2500\u2500 \u2500\u2500\u23a5\n",
|
||||
"\u23a28 3 2 \u23a5\n",
|
||||
"\u23a2 \u23a5\n",
|
||||
"\u23a2 3 2 \u23a5\n",
|
||||
"\u23a2T T \u23a5\n",
|
||||
"\u23a2\u2500\u2500 \u2500\u2500 T \u23a5\n",
|
||||
"\u23a36 2 \u23a6"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 72
|
||||
"prompt_number": 5
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
{
|
||||
"metadata": {
|
||||
"name": "",
|
||||
"signature": "sha256:ac5d16f13341646cfbb0fb366e162a56628ec5cb0c19001da7a7fb23105214b0"
|
||||
"signature": "sha256:a2e52b61eda730d642e07ed11f2d1d9c5d578591c7ac587dc7afee8425ce3e4d"
|
||||
},
|
||||
"nbformat": 3,
|
||||
"nbformat_minor": 0,
|
||||
@ -625,10 +625,12 @@
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"def sigma_points (X, P, kappa):\n",
|
||||
" \"\"\" Computes the sigma points and weights for an unscented Kalman filter\n",
|
||||
" given the mean and covariance of the filter.\n",
|
||||
" kappa is an arbitrary constant\n",
|
||||
" constant. Returns tuple of the sigma points and weights.\n",
|
||||
" \"\"\" Computes the sigma points and weights for an unscented \n",
|
||||
" Kalman filter given the mean and covariance of the filter.\n",
|
||||
" \n",
|
||||
" kappa is an arbitraryconstant. \n",
|
||||
" \n",
|
||||
" Returns tuple of the sigma points and weights.\n",
|
||||
"\n",
|
||||
" Works with both scalar and array inputs:\n",
|
||||
" sigma_points (5, 9, 2) # mean 5, covariance 9\n",
|
||||
@ -645,8 +647,8 @@
|
||||
" Returns\n",
|
||||
" -------\n",
|
||||
" sigmas : np.array, of size (n, 2n+1)\n",
|
||||
" Two dimensional array of sigma points. Each column contains all of\n",
|
||||
" the sigmas for one dimension in the problem space.\n",
|
||||
" Two dimensional array of sigma points. Each column contains\n",
|
||||
" all of the sigmas for one dimension in the problem space.\n",
|
||||
"\n",
|
||||
" Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}\n",
|
||||
"\n",
|
||||
@ -733,7 +735,9 @@
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"def unscented_transform (Xi, W, NoiseCov=None):\n",
|
||||
" \"\"\" computes the unscented transform of a set of signma points and weights.\n",
|
||||
" \"\"\" computes the unscented transform of a set of signma \n",
|
||||
" points and weights.\n",
|
||||
" \n",
|
||||
" returns the mean and covariance in a tuple\n",
|
||||
" \"\"\"\n",
|
||||
" kmax,n = Xi.shape\n",
|
||||
|
||||
File diff suppressed because one or more lines are too long
@ -1,7 +1,7 @@
|
||||
{
|
||||
"metadata": {
|
||||
"name": "",
|
||||
"signature": "sha256:aa9b4df3b3595ff9a790185720fa062f01b70b83d6266449e2fba5baecb5af02"
|
||||
"signature": "sha256:5ca766a2f349f8f9ea63fc2504c164a60dfe8bddd92c84cf7387db12cd8f1c4a"
|
||||
},
|
||||
"nbformat": 3,
|
||||
"nbformat_minor": 0,
|
||||
@ -302,9 +302,7 @@
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"import sympy\n",
|
||||
"from sympy import init_printing\n",
|
||||
"#from sympy.interactive import printing\n",
|
||||
"init_printing(use_latex='mathjax')\n",
|
||||
"sympy.init_printing(use_latex='mathjax')\n",
|
||||
"\n",
|
||||
"phi, x = sympy.symbols('\\phi, x')\n",
|
||||
"phi"
|
||||
|
||||
Loading…
Reference in New Issue
Block a user