Expanded ILS coverage.
Fixed notation for a priori and a posteriori variables. Explained how to compute H matrix by linearizing.
This commit is contained in:
Roger Labbe committed
1 parent
aa7ab29afa
commit
08328f0f0b
2 files changed
+133
-107
No files matched your search
+114
-106
@@ -1,7 +1,7 @@
|
||||
{
|
||||
"metadata": {
|
||||
"name": "",
|
||||
"signature": "sha256:3e8e9b87a74764e36232074e47d0a3ce000af637924cfb9109a5aa769cefbd65"
|
||||
"signature": "sha256:816c4af5667b2362a00c0592827f2972df778d9fb1e9947c284f686761053b42"
|
||||
},
|
||||
"nbformat": 3,
|
||||
"nbformat_minor": 0,
|
||||
@@ -261,7 +261,7 @@
|
||||
"output_type": "pyout",
|
||||
"prompt_number": 1,
|
||||
"text": [
|
||||
"<IPython.core.display.HTML at 0x7f95df102518>"
|
||||
"<IPython.core.display.HTML at 0x7f4c077034e0>"
|
||||
]
|
||||
}
|
||||
],
|
||||
@@ -738,7 +738,7 @@
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAxYAAAGaCAYAAACSU9UtAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x7f95bb24c2b0>"
|
||||
"<matplotlib.figure.Figure at 0x7f4c076f47f0>"
|
||||
]
|
||||
}
|
||||
],
|
||||
@@ -1512,7 +1512,7 @@
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAyIAAAGNCAYAAADtmMVMAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x7f95b961a390>"
|
||||
"<matplotlib.figure.Figure at 0x7f4be1c442b0>"
|
||||
]
|
||||
},
|
||||
{
|
||||
@@ -1537,9 +1537,11 @@
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"The previous section took a somewhat naive approach to integrating several sensor measurements into the filter. Let's work with a real world scenerio, albeit somewhat simplified. The GPS system is designed so that at least 6 satellites are in view at any time, assuming no line of sight blockage, at any point on the globe. The GPS receiver knows the location of the satellites in the sky relative to the Earth. At each epoch (instant in time) the receiver gets a signal from each satellite from which it can derive the *pseudorange* to the satellite. In more detail, the GPS receiver gets a signal identifying the satellite along with the time stamp of when the signal was transmitted. The GPS satellite has an atomic clock onboard so this timestamp is extremely accurate. The signal travels at the speed of light, which is constant in a vacuum, so in theory the GPS should be able to produce an extremely accurate distance measurement to the measurement by measuring how long the signal took to reach the receiver. There are several problems with that. First, the signal is not travelling through a vacuum, but through the atmosphere. The atmosphere causes the signal to bend, so it is not travelling in a straight line. This causes the signal to take longer to reach the receiver than theory suggests. Second, the on board clock on the GPS *receiver* is not very accurate, so deriving an exact time duration is nontrivial. Third, in many environments the signal can bounce off of buildings, trees, and other objects, causing either a longer path or *multipaths*, in which case the receive receives both the original signal from space and the reflected signals. \n",
|
||||
"A broad category of use for the Kalman filter is *sensor fusion*. For example, we might have a position sensor and a velocity sensor, and we want to combine the data from both to find an optimal estimate of state. In this section we will discuss a different case, where we have multiple sensors providing the same type of measurement. \n",
|
||||
"\n",
|
||||
"In the figure below I have shown how three pseudoranges might intersect in a 2D space. The width of the circle is meant to illustrate the measurement error for each pseudorange."
|
||||
" The Global Positioning System (GPS) is designed so that at least 6 satellites are in view at any time at any point on the globe. The GPS receiver knows the location of the satellites in the sky relative to the Earth. At each epoch (instant in time) the receiver gets a signal from each satellite from which it can derive the *pseudorange* to the satellite. In more detail, the GPS receiver gets a signal identifying the satellite along with the time stamp of when the signal was transmitted. The GPS satellite has an atomic clock onboard so this timestamp is extremely accurate. The signal travels at the speed of light, which is constant in a vacuum, so in theory the GPS should be able to produce an extremely accurate distance measurement to the measurement by measuring how long the signal took to reach the receiver. There are several problems with that. First, the signal is not travelling through a vacuum, but through the atmosphere. The atmosphere causes the signal to bend, so it is not travelling in a straight line. This causes the signal to take longer to reach the receiver than theory suggests. Second, the on board clock on the GPS *receiver* is not very accurate, so deriving an exact time duration is nontrivial. Third, in many environments the signal can bounce off of buildings, trees, and other objects, causing either a longer path or *multipaths*, in which case the receive receives both the original signal from space and the reflected signals. \n",
|
||||
"\n",
|
||||
"Let's look at this graphically. I will due this in 2D just to make it easier to graph and see, but of course this will generalize to three dimensions. We know the position of each satellite and the range to each (the range is called the *pseudorange*; we will discuss why later). We cannot measure the range exactly, so there is noise associated with the measurement, which I have depicted with the thickness of the lines. Here is an example of four pseudorange readings from four satellites. I positioned them in a configuration which is unlikely for the actual GPS constellation merely to make the intersections easy to visualize. Also, the amount of error shown is not to scale with the distances, again to make it easier to see."
|
||||
]
|
||||
},
|
||||
{
|
||||
@@ -1548,7 +1550,7 @@
|
||||
"input": [
|
||||
"import ukf_internal\n",
|
||||
"with book_format.figsize(10,6):\n",
|
||||
" ukf_internal.show_three_gps()"
|
||||
" ukf_internal.show_four_gps()"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
@@ -1556,27 +1558,29 @@
|
||||
{
|
||||
"metadata": {},
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAp8AAAGNCAYAAABXHhoFAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAp8AAAGNCAYAAABXHhoFAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x7f95b9534f60>"
|
||||
"<matplotlib.figure.Figure at 0x7f4be1b5f160>"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 17
|
||||
"prompt_number": 16
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"So we need to solve for the 3 dimensions of space, and 1 dimension of time. That is 4 unknowns, so in theory with 4 satellites we have all the information we need. However, we normally have at least 6 satellites in view, and often more than 6. This means the system is *overdetermined*. Finally, because of the noise in the measurements none of pseudoranges intersect exactly.\n",
|
||||
"In 2D two measurements are typically enough to determine a unique solution. There are two intersections of the range circles, but usually the second intersection is not physically realizable (it is in space, or under ground). However, with GPS we also need to solve for time, so we would need a third measurement to get a 2D position.\n",
|
||||
"\n",
|
||||
"Probably the most common approach used by GPS receivers to find the position is the *iterative least squares* algorithm, commonly abbreviated ILS. As you know, if the errors are Gaussian then the least squares algorithm finds the optimal solution. In other words, we want to minimize the square of the residuals for an overdetermined system.\n",
|
||||
"However, since GPS is a 3D system we need to solve for the 3 dimensions of space, and 1 dimension of time. That is 4 unknowns, so in theory with 4 satellites we have all the information we need. However, we normally have at least 6 satellites in view, and often more than 6. This means the system is *overdetermined*. Finally, because of the noise in the measurements none of pseudoranges intersect exactly.\n",
|
||||
"\n",
|
||||
"If you are well versed in linear algebra you know that this an extremely common problem in scientific computing, and that there are various techniques for solving overdetermied systems. Probably the most common approach used by GPS receivers to find the position is the *iterative least squares* algorithm, commonly abbreviated ILS. As you know, if the errors are Gaussian then the least squares algorithm finds the optimal solution. In other words, we want to minimize the square of the residuals for an overdetermined system.\n",
|
||||
"\n",
|
||||
"Let's start with some definitions which should be familar to you. First, we define the innovation as \n",
|
||||
"\n",
|
||||
"$$\\delta \\mathbf{z}= \\mathbf{z} - h(\\mathbf{x}^-)$$\n",
|
||||
"$$\\delta \\mathbf{z}^-= \\mathbf{z} - h(\\mathbf{x}^-)$$\n",
|
||||
"\n",
|
||||
"where $\\mathbf{z}$ is the measurement, $h(\\bullet)$ is the measurement function, and $\\delta \\mathbf{z}$ is the innovation, which we abbreviate as $y$ in FilterPy. I don't use the $\\mathbf{x}^-$ symbology often, but it is the prediction for the state variable. In other words, this is just the equation $\\mathbf{y} = \\mathbf{z} - \\mathbf{Hx}$ in the linear Kalman filter's update step.\n",
|
||||
"where $\\mathbf{z}$ is the measurement, $h(\\bullet)$ is the measurement function, and $\\delta \\mathbf{z}^-$ is the innovation, which we abbreviate as $y$ in FilterPy. I don't use the $\\mathbf{x}^-$ symbology often, but it is the prediction for the state variable. In other words, this is just the equation $\\mathbf{y} = \\mathbf{z} - \\mathbf{Hx}$ in the linear Kalman filter's update step.\n",
|
||||
"\n",
|
||||
"Next, the *measurement residual* is\n",
|
||||
"\n",
|
||||
@@ -1584,6 +1588,14 @@
|
||||
"\n",
|
||||
"I don't use the plus superscript much because I find it quickly makes the equations unreadable, but $\\mathbf{x}^+$ it is just the *a posteriori* state estimate, which is just the predicted or unknown future state. In other words, the predict step of the linear Kalman filter computes this value. Here it is stands for the value of x which the ILS algorithm will compute on each iteration.\n",
|
||||
"\n",
|
||||
"These equations give us the following linear algebra equation:\n",
|
||||
"\n",
|
||||
"$$\\delta \\mathbf{z}^- = \\mathbf{H}\\delta \\mathbf{x} + \\delta \\mathbf{z}^+$$\n",
|
||||
"\n",
|
||||
"$\\mathbf{H}$ is our measurement function, defined as\n",
|
||||
"\n",
|
||||
"$$\\mathbf{H} = \\frac{d\\mathbf{h}}{d\\mathbf{x}} = \\frac{d\\mathbf{z}}{d\\mathbf{x}}$$\n",
|
||||
"\n",
|
||||
"We find the minimum of an equation by taking the derivative and setting it to zero. In this case we want to minimize the square of the residuals, so our equation is"
|
||||
]
|
||||
},
|
||||
@@ -1591,26 +1603,26 @@
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"$$ \\frac{\\partial}{\\partial \\mathbf{x}}({\\delta \\mathbf{z}}^\\mathsf{T}\\delta \\mathbf{z}) = 0,$$\n",
|
||||
"$$ \\frac{\\partial}{\\partial \\mathbf{x}}({\\delta \\mathbf{z}^+}^\\mathsf{T}\\delta \\mathbf{z}^+) = 0,$$\n",
|
||||
"\n",
|
||||
"where\n",
|
||||
"\n",
|
||||
"$$\\delta \\mathbf{z}=\\delta \\mathbf{z}^- - \\mathbf{H}\\delta \\mathbf{x}.$$\n",
|
||||
"$$\\delta \\mathbf{z}^+=\\delta \\mathbf{z}^- - \\mathbf{H}\\delta \\mathbf{x}.$$\n",
|
||||
"\n",
|
||||
"Here I have switched to using the matrix $\\mathbf{H}$ as the measurement function. We want to use linear algebra to peform the ILS, so for each step we will have to compute the matrix $\\mathbf{H}$ which corresponds to $h(\\mathbf{x^-})$ during each iteration.\n",
|
||||
"Here I have switched to using the matrix $\\mathbf{H}$ as the measurement function. We want to use linear algebra to peform the ILS, so for each step we will have to compute the matrix $\\mathbf{H}$ which corresponds to $h(\\mathbf{x^-})$ during each iteration. $h(\\bullet)$ is usually nonlinear for these types of problems so you will have to linearize it at each step (more about this soon).\n",
|
||||
"\n",
|
||||
"For various reasons you may want to weigh some measurement more than others. For example, the geometry of the problem might favor orthogonal measurements, or some measurements may be more noisy than others. We can do that with the equation\n",
|
||||
"\n",
|
||||
"$$ \\frac{\\partial}{\\partial \\mathbf{x}}({\\delta \\mathbf{z}}^\\mathsf{T}\\mathbf{W}\\delta \\mathbf{z}) = 0$$\n",
|
||||
"$$ \\frac{\\partial}{\\partial \\mathbf{x}}({\\delta \\mathbf{z}^+}^\\mathsf{T}\\mathbf{W}\\delta \\mathbf{z}^+) = 0$$\n",
|
||||
"\n",
|
||||
"If we solve the first equation for ${\\delta \\mathbf{x}}$ (the derivation is shown in the next section) we get\n",
|
||||
"\n",
|
||||
"$${\\delta \\mathbf{x}} = {{(\\mathbf{H}^\\mathsf{T}\\mathbf{H})^{-1}}\\mathbf{H}^\\mathsf{T} \\delta \\mathbf{z}}\n",
|
||||
"$${\\delta \\mathbf{x}} = {{(\\mathbf{H}^\\mathsf{T}\\mathbf{H})^{-1}}\\mathbf{H}^\\mathsf{T} \\delta \\mathbf{z}^-}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"And the second equation yields\n",
|
||||
"\n",
|
||||
"$${\\delta \\mathbf{x}} = {{(\\mathbf{H}^\\mathsf{T}\\mathbf{WH})^{-1}}\\mathbf{H}^\\mathsf{T}\\mathbf{W} \\delta \\mathbf{z}}\n",
|
||||
"$${\\delta \\mathbf{x}} = {{(\\mathbf{H}^\\mathsf{T}\\mathbf{WH})^{-1}}\\mathbf{H}^\\mathsf{T}\\mathbf{W} \\delta \\mathbf{z}^-}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"Since the equations are overdetermined we cannot solve these equations exactly so we use an iterative approach. An initial guess for the position is made, and this guess is used to compute for $\\delta \\mathbf{x}$ via the equation above. $\\delta \\mathbf{x}$ is added to the intial guess, and this new state is fed back into the equation to produce another $\\delta \\mathbf{x}$. We iterate in this manner until the difference in the measurement residuals is suitably small."
|
||||
@@ -1630,7 +1642,7 @@
|
||||
"source": [
|
||||
"I will implement the ILS in code, but first let's derive the equation for $\\delta \\mathbf{x}$. You can skip the derivation if you want, but it is somewhat instructive and not too hard if you know basic linear algebra and partial differential equations.\n",
|
||||
"\n",
|
||||
"Substituting $\\delta \\mathbf{z}=\\delta \\mathbf{z}^- - \\mathbf{H}\\delta \\mathbf{x}$ into the partial differential equation we get\n",
|
||||
"Substituting $\\delta \\mathbf{z}^+=\\delta \\mathbf{z}^- - \\mathbf{H}\\delta \\mathbf{x}$ into the partial differential equation we get\n",
|
||||
"\n",
|
||||
"$$ \\frac{\\partial}{\\partial \\mathbf{x}}(\\delta \\mathbf{z}^- -\\mathbf{H} \\delta \\mathbf{x})^\\mathsf{T}(\\delta \\mathbf{z}^- - \\mathbf{H} \\delta \\mathbf{x})=0$$\n",
|
||||
"\n",
|
||||
@@ -1639,8 +1651,13 @@
|
||||
"$$ \\frac{\\partial}{\\partial \\mathbf{x}}({\\delta \\mathbf{x}}^\\mathsf{T}\\mathbf{H}^\\mathsf{T}\\mathbf{H}\\delta \\mathbf{x} - \n",
|
||||
"{\\delta \\mathbf{x}}^\\mathsf{T}\\mathbf{H}^\\mathsf{T}\\delta \\mathbf{z}^- - \n",
|
||||
"{\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H}\\delta \\mathbf{x} +\n",
|
||||
"{\\delta \\mathbf{z}^-}^\\mathsf{T}\\delta \\mathbf{z}^-)=0$$\n",
|
||||
"\n",
|
||||
"{\\delta \\mathbf{z}^-}^\\mathsf{T}\\delta \\mathbf{z}^-)=0$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"We know that \n",
|
||||
"\n",
|
||||
"$$\\frac{\\partial \\mathbf{A}^\\mathsf{T}\\mathbf{B}}{\\partial \\mathbf{B}} = \\frac{\\partial \\mathbf{B}^\\mathsf{T}\\mathbf{A}}{\\partial \\mathbf{B}} = \\mathbf{A}^\\mathsf{T}$$\n",
|
||||
@@ -1721,16 +1738,40 @@
|
||||
"source": [
|
||||
"Our goal is to implement an iterative solution to \n",
|
||||
"$${\\delta \\mathbf{x}} = {{(\\mathbf{H}^\\mathsf{T}\\mathbf{H})^{-1}}\\mathbf{H}^\\mathsf{T} \\delta \\mathbf{z}^-}\n",
|
||||
"$$"
|
||||
"$$\n",
|
||||
"\n",
|
||||
"First, we have to compute $\\mathbf{H}$, where $\\mathbf{H} = d\\mathbf{z}/d\\mathbf{x}$. Just to keep the example small so the results are easier to interpret we will do this in 2D. Therefore for $n$ satellites $\\mathbf{H}$ expands to\n",
|
||||
"\n",
|
||||
"$$\\mathbf{H} = \\begin{bmatrix}\n",
|
||||
"\\frac{\\partial p_1}{\\partial x_1} & \\frac{\\partial p_1}{\\partial y_1} \\\\\n",
|
||||
"\\frac{\\partial p_2}{\\partial x_2} & \\frac{\\partial p_2}{\\partial y_2} \\\\\n",
|
||||
"\\vdots & \\vdots \\\\\n",
|
||||
"\\frac{\\partial p_n}{\\partial x_n} & \\frac{\\partial p_n}{\\partial y_n}\n",
|
||||
"\\end{bmatrix}$$\n",
|
||||
"\n",
|
||||
"We will linearize $\\mathbf{H}$ by computing the partial for $x$ as\n",
|
||||
"\n",
|
||||
"$$ \\frac{estimated\\_x\\_position - satellite\\_x\\_position}{estimated\\_range\\_to\\_satellite}$$\n",
|
||||
"\n",
|
||||
"The equation for $y$ just subtitutes $y$ for $x$.\n",
|
||||
"\n",
|
||||
"Then the algorithm is as follows.\n",
|
||||
"\n",
|
||||
" def ILS:\n",
|
||||
" guess position\n",
|
||||
" while not converged:\n",
|
||||
" compute range to satellites for current estimated position\n",
|
||||
" compute H linearized at estimated position\n",
|
||||
" compute new estimate delta from (H^T H)'H^T dz\n",
|
||||
" new estimate = current estimate + estimate delta\n",
|
||||
" check for convergence\n",
|
||||
" "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"# this code needs exposition. Just putting here without\n",
|
||||
"# explanation for the moment.\n",
|
||||
"\n",
|
||||
"import numpy as np\n",
|
||||
"from numpy.linalg import norm, inv\n",
|
||||
"from numpy.random import randn\n",
|
||||
@@ -1741,19 +1782,19 @@
|
||||
"user_pos = np.array([800, 200])\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"transmitter_pos = np.asarray(\n",
|
||||
"sat_pos = np.asarray(\n",
|
||||
" [[0, 1000],\n",
|
||||
" [0, -1000],\n",
|
||||
" [500, 500]], dtype=float)\n",
|
||||
"\n",
|
||||
"def transmitter_range(pos, transmitter_pos):\n",
|
||||
"def satellite_range(pos, sat_pos):\n",
|
||||
" \"\"\" Compute distance between position 'pos' and the list of positions\n",
|
||||
" in transmitter_pos\"\"\"\n",
|
||||
" in sat_pos\"\"\"\n",
|
||||
"\n",
|
||||
" N = len(transmitter_pos)\n",
|
||||
" N = len(sat_pos)\n",
|
||||
" rng = np.zeros(N)\n",
|
||||
"\n",
|
||||
" diff = np.asarray(pos) - transmitter_pos\n",
|
||||
" diff = np.asarray(pos) - sat_pos\n",
|
||||
"\n",
|
||||
" for i in range(N):\n",
|
||||
" rng[i] = norm(diff[i])\n",
|
||||
@@ -1761,16 +1802,27 @@
|
||||
" return norm(diff, axis=1)\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"def hx_ils(pos, t_pos, r_est):\n",
|
||||
" N = len(t_pos)\n",
|
||||
"def hx_ils(pos, sat_pos, range_est):\n",
|
||||
" \"\"\" compute measurement function where\n",
|
||||
" pos : array_like \n",
|
||||
" 2D current estimated position. e.g. (23, 45)\n",
|
||||
" \n",
|
||||
" sat_pos : array_like of 2D positions\n",
|
||||
" position of each satellite e.g. [(0,100), (100,0)]\n",
|
||||
" \n",
|
||||
" range_est : array_like of floats\n",
|
||||
" range to each satellite\n",
|
||||
" \"\"\"\n",
|
||||
" \n",
|
||||
" N = len(sat_pos)\n",
|
||||
" H = np.zeros((N, 2))\n",
|
||||
" for j in range(N):\n",
|
||||
" H[j,0] = -(t_pos[j,0] - pos[0]) / r_est[j]\n",
|
||||
" H[j,1] = -(t_pos[j,1] - pos[1]) / r_est[j]\n",
|
||||
" H[j,0] = (pos[0] - sat_pos[j,0]) / range_est[j]\n",
|
||||
" H[j,1] = (pos[1] - sat_pos[j,1]) / range_est[j]\n",
|
||||
" return H\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"def lop_ils(zs, t_pos, pos_est, hx, eps=1.e-6):\n",
|
||||
"def lop_ils(zs, sat_pos, pos_est, hx, eps=1.e-6):\n",
|
||||
" \"\"\" iteratively estimates the solution to a set of measurement, given\n",
|
||||
" known transmitter locations\"\"\"\n",
|
||||
" pos = np.array(pos_est)\n",
|
||||
@@ -1778,10 +1830,10 @@
|
||||
" with book_format.numpy_precision(precision=4):\n",
|
||||
" converged = False\n",
|
||||
" for i in range(20):\n",
|
||||
" r_est = transmitter_range(pos, t_pos)\n",
|
||||
" r_est = satellite_range(pos, sat_pos)\n",
|
||||
" print('iteration:', i)\n",
|
||||
"\n",
|
||||
" H=hx(pos, t_pos, r_est) \n",
|
||||
" H=hx(pos, sat_pos, r_est) \n",
|
||||
" Hinv = inv(dot(H.T, H)).dot(H.T)\n",
|
||||
"\n",
|
||||
" #update position estimate\n",
|
||||
@@ -1789,12 +1841,8 @@
|
||||
" print('innovation', y)\n",
|
||||
"\n",
|
||||
" Hy = np.dot(Hinv, y)\n",
|
||||
"\n",
|
||||
" pos = pos + Hy\n",
|
||||
" print('pos ', pos)\n",
|
||||
"\n",
|
||||
" print()\n",
|
||||
" print()\n",
|
||||
" print('pos {}\\n\\n'.format(pos))\n",
|
||||
"\n",
|
||||
" if max(abs(Hy)) < eps:\n",
|
||||
" converged = True\n",
|
||||
@@ -1803,10 +1851,10 @@
|
||||
" return pos, converged\n",
|
||||
"\n",
|
||||
"# compute measurement of where you are with respect to each sensor\n",
|
||||
"rz= transmitter_range(user_pos, transmitter_pos)\n",
|
||||
"rz= satellite_range(user_pos, sat_pos)\n",
|
||||
"\n",
|
||||
"pos, converted = lop_ils(rz, transmitter_pos, (900,90), hx=hx_ils)\n",
|
||||
"print('Iterated solution: ', pos)\n"
|
||||
"pos, converted = lop_ils(rz, sat_pos, (900,90), hx=hx_ils)\n",
|
||||
"print('Iterated solution: ', pos)"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
@@ -1817,29 +1865,29 @@
|
||||
"text": [
|
||||
"iteration: 0\n",
|
||||
"innovation [-148.512 28.6789 -148.5361]\n",
|
||||
"pos [ 805.4175 205.2868]\n",
|
||||
"pos [ 805.4175 205.2868]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 1\n",
|
||||
"innovation [-0.1177 -7.4049 -0.1599]\n",
|
||||
"pos [ 800.04 199.9746]\n",
|
||||
"pos [ 800.04 199.9746]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 2\n",
|
||||
"innovation [-0.0463 -0.001 -0.0463]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 3\n",
|
||||
"innovation [-0. -0. -0.]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"Iterated solution: [ 800. 200.]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 19
|
||||
"prompt_number": 17
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@@ -1850,14 +1898,14 @@
|
||||
"$$\\mathbf{y} = \\mathbf{z} - \\mathbf{Hx}\\\\\n",
|
||||
"\\mathbf{x} = \\mathbf{x} + \\mathbf{Ky}$$\n",
|
||||
"\n",
|
||||
"but for the Kalman gain equalling one. You can see that despite the very inaccurate initial guess (900, 90) the computed value for $\\mathbf{x}$ was very close to the actual value of (800,100). However, it was not perfect. But after three iterations the ILS algorithm was able to find the exact answer. So hopefully it is clear why we use ILS instead of doing the sensor fusion with the Kalman filter - it gives a better result. Of course, we started with a very inaccurate guess; what if the guess was better?"
|
||||
"where the Kalman gain equals one. You can see that despite the very inaccurate initial guess (900, 90) the computed value for $\\mathbf{x}$, (805.4, 205.3), was very close to the actual value of (800, 200). However, it was not perfect. But after three iterations the ILS algorithm was able to find the exact answer. So hopefully it is clear why we use ILS instead of doing the sensor fusion with the Kalman filter - it gives a better result. Of course, we started with a very inaccurate guess; what if the guess was better?"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"pos, converted = lop_ils(rz, transmitter_pos, (801, 201), hx=hx_ils)\n",
|
||||
"pos, converted = lop_ils(rz, sat_pos, (801, 201), hx=hx_ils)\n",
|
||||
"print('Iterated solution: ', pos)"
|
||||
],
|
||||
"language": "python",
|
||||
@@ -1869,24 +1917,24 @@
|
||||
"text": [
|
||||
"iteration: 0\n",
|
||||
"innovation [-0.0009 -1.3868 -0.0024]\n",
|
||||
"pos [ 800.0014 199.9991]\n",
|
||||
"pos [ 800.0014 199.9991]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 1\n",
|
||||
"innovation [-0.0016 -0. -0.0016]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 2\n",
|
||||
"innovation [-0. -0. -0.]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"Iterated solution: [ 800. 200.]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 20
|
||||
"prompt_number": 18
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@@ -1905,7 +1953,7 @@
|
||||
"nrz = []\n",
|
||||
"for z in rz:\n",
|
||||
" nrz.append(z + randn())\n",
|
||||
"pos, converted = lop_ils(nrz, transmitter_pos, (601,198.3), hx=hx_ils)\n",
|
||||
"pos, converted = lop_ils(nrz, sat_pos, (601,198.3), hx=hx_ils)\n",
|
||||
"print('Iterated solution: ', pos)"
|
||||
],
|
||||
"language": "python",
|
||||
@@ -1917,34 +1965,34 @@
|
||||
"text": [
|
||||
"iteration: 0\n",
|
||||
"innovation [ 129.8823 100.461 107.5398]\n",
|
||||
"pos [ 831.4474 186.1222]\n",
|
||||
"pos [ 831.4474 186.1222]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 1\n",
|
||||
"innovation [-31.6446 -7.4837 -30.7861]\n",
|
||||
"pos [ 800.3284 198.8076]\n",
|
||||
"pos [ 800.3284 198.8076]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 2\n",
|
||||
"innovation [-0.6041 -0.3813 0.3569]\n",
|
||||
"pos [ 799.948 198.6026]\n",
|
||||
"pos [ 799.948 198.6026]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 3\n",
|
||||
"innovation [-0.4803 0.0004 0.4802]\n",
|
||||
"pos [ 799.9476 198.6025]\n",
|
||||
"pos [ 799.9476 198.6025]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 4\n",
|
||||
"innovation [-0.4802 0.0007 0.4803]\n",
|
||||
"pos [ 799.9476 198.6025]\n",
|
||||
"pos [ 799.9476 198.6025]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"Iterated solution: [ 799.9475854 198.60245871]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 21
|
||||
"prompt_number": 19
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@@ -1952,49 +2000,9 @@
|
||||
"source": [
|
||||
"Here we can see that the noise means that we no longer find the exact solution but we are still able to quickly converge onto a more accurate solution than the first iteration provides.\n",
|
||||
"\n",
|
||||
"This is far from a complete coverage of the iterated least squares algorithm, let alone methods used in GNSS to compute positions from GPS pseudoranges. You will find a number of approaches in the literature, including QR decomposition, using SVD, and other techniques to solve the overdetermined system. For a nontrivial task you will have to survey the literature and perhaps invent a technique for your specifc sensor configuration. "
|
||||
"This is far from a complete coverage of the iterated least squares algorithm, let alone methods used in GNSS to compute positions from GPS pseudoranges. You will find a number of approaches in the literature, including QR decomposition, SVD, and other techniques to solve the overdetermined system. For a nontrivial task you will have to survey the literature and perhaps design your algorithm depending on your specifc sensor configuration, the amounts of noise, your accuracy requirements, and the amount of computation you can afford to do."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 2,
|
||||
|
||||
+19
-1
@@ -59,6 +59,24 @@ def show_three_gps():
|
||||
plt.show()
|
||||
|
||||
|
||||
|
||||
|
||||
def show_four_gps():
|
||||
circle1=plt.Circle((-4,2),5,color='#004080',fill=False,linewidth=20, alpha=.7)
|
||||
circle2=plt.Circle((5.5,1),5,color='#E24A33', fill=False, linewidth=8, alpha=.7)
|
||||
circle3=plt.Circle((0,-3),6,color='#534543',fill=False, linewidth=13, alpha=.7)
|
||||
circle4=plt.Circle((0,8),5,color='#214513',fill=False, linewidth=13, alpha=.7)
|
||||
|
||||
fig = plt.gcf()
|
||||
ax = fig.gca()
|
||||
|
||||
ax.add_patch(circle1)
|
||||
ax.add_patch(circle2)
|
||||
ax.add_patch(circle3)
|
||||
ax.add_patch(circle4)
|
||||
|
||||
plt.axis('equal')
|
||||
plt.show()
|
||||
def show_sigma_transform():
|
||||
fig = plt.figure()
|
||||
ax=fig.gca()
|
||||
@@ -174,7 +192,7 @@ def show_sigmas_for_2_kappas():
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
show_three_gps()
|
||||
show_four_gps()
|
||||
#show_sigma_transform()
|
||||
#show_sigma_selections()
|
||||
|
||||
Reference in new issue
Block a user