Moved ILS to the Kalman Math chapter.
This commit is contained in:
Roger Labbe committed
1 parent
7c5bdccaa1
commit
aa7ab29afa
2 files changed
+478
-477
No files matched your search
+476
-4
@@ -1,7 +1,7 @@
|
||||
{
|
||||
"metadata": {
|
||||
"name": "",
|
||||
"signature": "sha256:85a87b3bc9daf34f345daefcbf63b738db291bad018d92551178c5b8f8bf909b"
|
||||
"signature": "sha256:3e8e9b87a74764e36232074e47d0a3ce000af637924cfb9109a5aa769cefbd65"
|
||||
},
|
||||
"nbformat": 3,
|
||||
"nbformat_minor": 0,
|
||||
@@ -29,6 +29,8 @@
|
||||
"input": [
|
||||
"#format the book\n",
|
||||
"%matplotlib inline\n",
|
||||
"%load_ext autoreload\n",
|
||||
"%autoreload 2\n",
|
||||
"from __future__ import division, print_function\n",
|
||||
"import matplotlib.pyplot as plt\n",
|
||||
"import book_format\n",
|
||||
@@ -259,7 +261,7 @@
|
||||
"output_type": "pyout",
|
||||
"prompt_number": 1,
|
||||
"text": [
|
||||
"<IPython.core.display.HTML at 0x7fa0fc79f320>"
|
||||
"<IPython.core.display.HTML at 0x7f95df102518>"
|
||||
]
|
||||
}
|
||||
],
|
||||
@@ -736,7 +738,7 @@
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAxYAAAGaCAYAAACSU9UtAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x7fa0fc79fbe0>"
|
||||
"<matplotlib.figure.Figure at 0x7f95bb24c2b0>"
|
||||
]
|
||||
}
|
||||
],
|
||||
@@ -1510,7 +1512,7 @@
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAyIAAAGNCAYAAADtmMVMAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\nAAALEgAACxIB0t1+/AAAIABJREFUeJzs3X9clfX9//EnP0V+iIocjgIiKgpi8XG6plY4E90sl9pK\n07Vs+ZmtCCNrLO0HNk2zmRKp/dpMpx+z1lxt1YpZrjRriblKEfyFv8BzVEQQBYRzzvePvp26UuSH\nwOFwHvfbzdtt53W93/C69s7iyXW9r8vL4XA4BAAAAACtyNvVDQAAAADwPAQRAAAAAK2OIAIAAACg\n1RFEAAAAALQ6gggAAACAVkcQAQAAANDqCCIAAAAAWl29QaS2tlZz5sxR79691bFjR/Xu3VuPPvqo\nbDabYdzcuXMVGRmpwMBAjRw5Unl5eYbj1dXVSktLU3h4uIKDgzV+/HgVFRU179kAAAAAcAv1BpEF\nCxbohRde0LPPPquCggI988wzWrFihRYuXOgcs2jRIi1ZskTLli3Ttm3bZDKZNHr0aFVUVDjHpKen\na8OGDVq/fr02b96s8vJyjRs3Tna7vWXODAAAAECb5VXfm9V/9rOfqVu3bnr55ZedtWnTpunUqVP6\nxz/+IYfDoR49emjmzJmaPXu2JKmqqkomk0mLFy/WjBkzVFZWJpPJpFWrVmnKlCmSpKNHjyomJkb/\n/Oc/NWbMmBY8RQAAAABtTb1XRMaOHasPPvhABQUFkqS8vDxt2rRJN9xwgySpsLBQVqvVECYCAgKU\nnJysrVu3SpK2b9+umpoaw5ioqCglJCQ4xwAAAADwHL71Dbjnnnt09OhRJSQkyNfXV7W1tXrkkUf0\nm9/8RpJksVgkSREREYZ5JpNJxcXFzjE+Pj4KCwszjImIiJDVanV+Lisru7yzAQAAAOASoaGhjRpf\nbxDJzs7Wyy+/rPXr1ysxMVE7duzQfffdp169eunOO++85FwvL69GNQMAAADAM9R7a9YTTzyhOXPm\naNKkSUpMTNRtt92mWbNmOTerm81mSTJc2fjm8zfHzGazbDabSkpKDGMsFotzDAAAAADPUe8VEYfD\nIW9vY17x9vbWN3vcY2NjZTablZOTo8GDB0v6erP6li1btHjxYknS4MGD5efnp5ycHMNm9fz8fA0f\nPvyi37exl3bgnnJzcyVJQ4YMcXEnaE2su+dhzT0Pa+6ZWHfPczlbK+oNIhMmTNCTTz6p2NhYDRgw\nQDt27NDSpUs1bdo0SV/ffpWenq4FCxYoPj5ecXFxmj9/vkJCQjR16lRJX4eK6dOnKyMjQyaTSV27\ndtWsWbOUlJSklJSUJjcPAAAAwD3VG0SWLl2qTp06KTU1VVarVd27d9eMGTP02GOPOcdkZGSosrJS\nqampKi0t1dChQ5WTk6OgoCDnmKysLPn6+mry5MmqrKxUSkqK1q5dyz4SAAAAwAPV+x6R1vTdSzvc\nmuUZuITrmVh3z8Oaex7W3DOx7p7ncn5+r3ezOgAAAAA0N4IIAAAAgFZHEAEAAADQ6ggiAAAAAFod\nQQQAAABAqyOIAAAAAGh1BBEAAAAArY4gAgAAAKDVEUQAAAAAtDqCCAAAAIBWRxABAAAA0OoIIgAA\nAABaHUEEAAAAQKsjiAAAAABodQQRAAAAAK2OIAIAAACg1RFEAAAAALQ6gggAAACAVkcQAQAAANDq\nCCIAAAAAWh1BBAAAAECrI4gAAAAAaHUEEQAAAACtjiACAAAAoNURRAAAAAC0OoIIAAAAgFZHEAEA\nAADQ6ggiAAAAAFodQQQAAABAqyOIAAAAAGh1BBEAAAAArY4gAgAAAKDVEUQAAAAAtDqCCAAAAIBW\nV28Q6dWrl7y9vS/4M27cOEmSw+HQ3LlzFRkZqcDAQI0cOVJ5eXmGr1FdXa20tDSFh4crODhY48eP\nV1FRUcucEQAAAIA2r94gsn37dlksFuefzz//XF5eXpo8ebIk6amnntKSJUu0bNkybdu2TSaTSaNH\nj1ZFRYXza6Snp2vDhg1av369Nm/erPLyco0bN052u73lzgwAAABAm1VvEAkLC5PJZHL+efvttxUa\nGqpJkybJ4XAoKytLs2fP1sSJE5WYmKjVq1frzJkzWrdunSSprKxMK1eu1OLFizVq1CgNGjRIa9as\n0ZdffqmNGze2+AkCAAAAaHsatUfE4XDoT3/6k2677TZ16NBBhYWFslqtGjNmjHNMQECAkpOTtXXr\nVklfX1GpqakxjImKilJCQoJzDAAAANqfLV9YNOAXe/X3LYdd3QraIN/GDP7Xv/6lgwcP6te//rUk\nyWKxSJIiIiIM40wmk4qLi51jfHx8FBYWZhgTEREhq9Va5/fKzc1tTGtwc6y3Z2LdPQ9r7nlYc8/0\nzbqnPuOv/INXaMLvpCt6f6n7J5RpYExHF3eH5hQXF9fkuY26IvLSSy/pqquu0hVXXFHvWC8vryY3\nBQAAAPe2dfdZfXXg258ZvzpwhXYeYn8wvtXgKyLHjx/X3//+d61YscJZM5vNkiSr1aqoqChn3Wq1\nOo+ZzWbZbDaVlJQYropYLBYlJyfX+f2GDBnS8LOA2/rmNyast2dh3T0Pa+55WHPP9N11/+XT+wzH\nIsOP6Kn0ZHl788vq9qSsrKzJcxt8RWTVqlUKCAjQlClTnLXY2FiZzWbl5OQ4a1VVVdqyZYuGDx8u\nSRo8eLD8/PwMY44ePar8/HznGAAAALQfq97Zr4LDfQy1x//3PCEEBg26IuJwOPTHP/5Rt956qwID\nA511Ly8vpaena8GCBYqPj1dcXJzmz5+vkJAQTZ06VZIUGhqq6dOnKyMjQyaTSV27dtWsWbOUlJSk\nlJSUljkrAAAAuITd7lDmS8YfMeOiD+iO6/vUMQOeqkFB5N///rf279/vfCTvd2VkZKiyslKpqakq\nLS3V0KFDlZOTo6CgIOeYrKws+fr6avLkyaqsrFRKSorWrl3LPhIAAIB25vWt53TkeE9DbdE9vlwN\nwQUaFERGjhwpm81W5/HMzExlZmbWedzf31/Z2dnKzs5ufIcAAABwC+dr7VqV09dQG9SvQBOS413U\nEdqyRj01CwAAAKjLnz+o1MmyHs7PXl42LZkZ6sKO0JYRRAAAAHDZzpw9r/X/HmCoXZNUoBGDuruo\nI7R1BBEAAABcttkvFKj8bDfnZx/vGmWnR1xiBjwdQQQAAACX5cTpSr38lnGD+k+GFigprlsdMwCC\nCAAAAC7TzKX7VFndyfnZz7dS2ek9LzEDIIgAAADgMhQcLtVfNxmflHXzyP3qHckmdVwaQQQAAABN\nlvp0kWptAc7PHTuUKSu97yVmAF8jiAAAAKBJNn9xTJu2G98RcsuILxXeuaOLOoI7IYgAAACgSWYu\nLZfD4eP83CXEohljAi4xA/gWQQQAAACNtn5job7Y289Qu/MnBfL348dLNAz/pAAAAKBR7HaHZj/n\nZajFmA/plquDXNQR3BFBBAAAAI2ydH2BDlliDLUn73HI29urjhnAhQgiAAAAaLCq6lotXNPFUEuK\n26PJo2Jd1BHcFUEEAAAADTb7hXydKjc5P3t52fTs/bwzBI1HEAEAAECDnDhdqZfejDbURg7O1zVJ\nZhd1BHdGEAEAAECDzFy6T+eqOjk/+/pUafkDkS7sCO6MIAIAAIB67TxQotc3GR/Xe8t1+9S/Z5c6\nZgCXRhABAABAvX7zB6tsNn/n56CAMmWl93VhR3B3BBEAAABc0lsfH9HWLxMMtdSbixTeuaOLOkJ7\nQBABAABAnex2h9KfqTHUzF2LNe9/413UEdoLgggAAADqlPVagQ4UGd8RMv+uSvn5+bioI7QXBBEA\nAABc1LmqGj2x2rgZPbH3Xt05jr0huHwEEQAAAFzUb5cVqPR7Ly9cNivEhR2hPSGIAAAA4AKHLeVa\n+XYvQ230VfkaMai7axpCu0MQAQAAwAXuXnxY1eeDnJ/9/c7puQd7urAjtDcEEQAAABh8stOi9z7t\nb6jdPvaAYnt0qmMG0HgEEQAAABjc/Ycy2R2+zs+dg0/q6Xv7X2IG0HgEEQAAADitfGufvtzXz1D7\n3S9PKiTIv44ZQNMQRAAAACBJqqqu1ZznAw21mO6H9NupvLwQzY8gAgAAAEnS757L1/FS41Oxnr3f\nR97eXi7qCO0ZQQQAAAA6ZDmjF9/sZagl/0+exl0d7ZqG0O4RRAAAAKAZiw5d8Ljelx6KdGFHaO8I\nIgAAAB5u47Yibdxm3Acy7foDiovu7KKO4AkIIgAAAB7Mbnco9ekqORw+zlrXTsf19L1sUEfLalAQ\nOXbsmKZNmyaTyaSOHTsqMTFRH330kWHM3LlzFRkZqcDAQI0cOVJ5eXmG49XV1UpLS1N4eLiCg4M1\nfvx4FRUVNd+ZAAAAoNGyXivQ3iO9DbXM6acVHOjnoo7gKeoNIqdPn9bVV18tLy8vvfPOO8rPz9ey\nZctkMpmcYxYtWqQlS5Zo2bJl2rZtm0wmk0aPHq2KigrnmPT0dG3YsEHr16/X5s2bVV5ernHjxslu\nt7fMmQEAAOCSzpw9r/kvhxlq8b32KfWmfnXMAJqPb30DnnrqKUVGRmrVqlXOWkxMjPN/OxwOZWVl\nafbs2Zo4caIkafXq1TKZTFq3bp1mzJihsrIyrVy5UqtWrdKoUaMkSWvWrFFMTIw2btyoMWPGNPNp\nAQAAoD5pWQU6XTHQ+dnLy6YXMoJ5XC9aRb1XRN544w1dddVVmjx5siIiIjRo0CAtX77cebywsFBW\nq9UQJgICApScnKytW7dKkrZv366amhrDmKioKCUkJDjHAAAAoPXsPHBK697ra6iNHbZb1yZ1r2MG\n0LzqvSJy4MABrVixQrNmzdKcOXO0Y8cOpaWlSZJSU1NlsVgkSREREYZ5JpNJxcXFkiSLxSLine truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x7fa0d7227e80>"
|
||||
"<matplotlib.figure.Figure at 0x7f95b961a390>"
|
||||
]
|
||||
},
|
||||
{
|
||||
@@ -1523,6 +1525,476 @@
|
||||
],
|
||||
"prompt_number": 15
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 2,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Iterative Least Squares for Sensor Fusion"
|
||||
]
|
||||
},
|
||||
{
|
||||
"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",
|
||||
"\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."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"import ukf_internal\n",
|
||||
"with book_format.figsize(10,6):\n",
|
||||
" ukf_internal.show_three_gps()"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"metadata": {},
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAp8AAAGNCAYAAABXHhoFAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x7f95b9534f60>"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 17
|
||||
},
|
||||
{
|
||||
"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",
|
||||
"\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",
|
||||
"\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",
|
||||
"\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",
|
||||
"$$\\delta \\mathbf{z}^+ = \\mathbf{z} - h(\\mathbf{x}^+)$$\n",
|
||||
"\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",
|
||||
"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"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"$$ \\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",
|
||||
"\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",
|
||||
"\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",
|
||||
"\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",
|
||||
"$$\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",
|
||||
"$$\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."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 3,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Derivation of ILS Equations (Optional)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"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",
|
||||
"\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",
|
||||
"which expands to\n",
|
||||
"\n",
|
||||
"$$ \\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",
|
||||
"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",
|
||||
"\n",
|
||||
"Therefore the third term can be computed as\n",
|
||||
"\n",
|
||||
"$$\\frac{\\partial}{\\partial \\mathbf{x}}{\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H}\\delta \\mathbf{x} = {\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H}$$\n",
|
||||
"\n",
|
||||
"and the second term as\n",
|
||||
"\n",
|
||||
"$$\\frac{\\partial}{\\partial \\mathbf{x}}{\\delta \\mathbf{x}}^\\mathsf{T}\\mathbf{H}^\\mathsf{T}\\delta \\mathbf{z}^-={\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H}$$\n",
|
||||
"\n",
|
||||
"We also know that\n",
|
||||
"$$\\frac{\\partial \\mathbf{B}^\\mathsf{T}\\mathbf{AB}}{\\partial \\mathbf{B}} = \\mathbf{B}^\\mathsf{T}(\\mathbf{A} + \\mathbf{A}^\\mathsf{T})$$\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"Therefore the first term becomes\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\begin{aligned}\n",
|
||||
"\\frac{\\partial}{\\partial \\mathbf{x}}{\\delta \\mathbf{x}}^\\mathsf{T}\\mathbf{H}^\\mathsf{T}\\mathbf{H}\\delta \\mathbf{x} &= {\\delta \\mathbf{x}}^\\mathsf{T}(\\mathbf{H}^\\mathsf{T}\\mathbf{H} + {\\mathbf{H}^\\mathsf{T}\\mathbf{H}}^\\mathsf{T})\\\\\n",
|
||||
"&= {\\delta \\mathbf{x}}^\\mathsf{T}(\\mathbf{H}^\\mathsf{T}\\mathbf{H} + \\mathbf{H}^\\mathsf{T}\\mathbf{H}) \\\\\n",
|
||||
"&= 2{\\delta \\mathbf{x}}^\\mathsf{T}\\mathbf{H}^\\mathsf{T}\\mathbf{H}\n",
|
||||
"\\end{aligned}$$\n",
|
||||
"\n",
|
||||
"Finally, the fourth term is\n",
|
||||
"\n",
|
||||
"$$ \\frac{\\partial}{\\partial \\mathbf{x}}\n",
|
||||
"{\\delta \\mathbf{z}^-}^\\mathsf{T}\\delta \\mathbf{z}^-=0$$\n",
|
||||
"\n",
|
||||
"Replacing the terms in the expanded partial differential equation gives us\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
" 2{\\delta \\mathbf{x}}^\\mathsf{T}\\mathbf{H}^\\mathsf{T}\\mathbf{H} -\n",
|
||||
" {\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H} - {\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H}\n",
|
||||
" =0\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"$${\\delta \\mathbf{x}}^\\mathsf{T}\\mathbf{H}^\\mathsf{T}\\mathbf{H} -\n",
|
||||
" {\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H} = 0$$\n",
|
||||
" \n",
|
||||
" $${\\delta \\mathbf{x}}^\\mathsf{T}\\mathbf{H}^\\mathsf{T}\\mathbf{H} =\n",
|
||||
" {\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H}$$\n",
|
||||
"\n",
|
||||
"Multiplying each side by $(\\mathbf{H}^\\mathsf{T}\\mathbf{H})^{-1}$ yields\n",
|
||||
"\n",
|
||||
"$${\\delta \\mathbf{x}}^\\mathsf{T} =\n",
|
||||
"{\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H}(\\mathbf{H}^\\mathsf{T}\\mathbf{H})^{-1}$$\n",
|
||||
"\n",
|
||||
"Taking the transpose of each side gives\n",
|
||||
"\n",
|
||||
"$${\\delta \\mathbf{x}} = ({{\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H}(\\mathbf{H}^\\mathsf{T}\\mathbf{H})^{-1}})^\\mathsf{T} \\\\\n",
|
||||
"={{(\\mathbf{H}^\\mathsf{T}\\mathbf{H})^{-1}}^T\\mathbf{H}^\\mathsf{T} \\delta \\mathbf{z}^-} \\\\\n",
|
||||
"={{(\\mathbf{H}^\\mathsf{T}\\mathbf{H})^{-1}}\\mathbf{H}^\\mathsf{T} \\delta \\mathbf{z}^-}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"For various reasons you may want to weigh some measurement more 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",
|
||||
"\n",
|
||||
"Replicating the math above with the added $\\mathbf{W}$ term results in\n",
|
||||
"\n",
|
||||
"$${\\delta \\mathbf{x}} = {{(\\mathbf{H}^\\mathsf{T}\\mathbf{WH})^{-1}}\\mathbf{H}^\\mathsf{T}\\mathbf{W} \\delta \\mathbf{z}^-}\n",
|
||||
"$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 3,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Implementing Iterative Least Squares"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"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",
|
||||
"$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"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",
|
||||
"from numpy import dot\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"np.random.seed(1234)\n",
|
||||
"user_pos = np.array([800, 200])\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"transmitter_pos = np.asarray(\n",
|
||||
" [[0, 1000],\n",
|
||||
" [0, -1000],\n",
|
||||
" [500, 500]], dtype=float)\n",
|
||||
"\n",
|
||||
"def transmitter_range(pos, transmitter_pos):\n",
|
||||
" \"\"\" Compute distance between position 'pos' and the list of positions\n",
|
||||
" in transmitter_pos\"\"\"\n",
|
||||
"\n",
|
||||
" N = len(transmitter_pos)\n",
|
||||
" rng = np.zeros(N)\n",
|
||||
"\n",
|
||||
" diff = np.asarray(pos) - transmitter_pos\n",
|
||||
"\n",
|
||||
" for i in range(N):\n",
|
||||
" rng[i] = norm(diff[i])\n",
|
||||
"\n",
|
||||
" return norm(diff, axis=1)\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"def hx_ils(pos, t_pos, r_est):\n",
|
||||
" N = len(t_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",
|
||||
" return H\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"def lop_ils(zs, t_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",
|
||||
"\n",
|
||||
" 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",
|
||||
" print('iteration:', i)\n",
|
||||
"\n",
|
||||
" H=hx(pos, t_pos, r_est) \n",
|
||||
" Hinv = inv(dot(H.T, H)).dot(H.T)\n",
|
||||
"\n",
|
||||
" #update position estimate\n",
|
||||
" y = zs - r_est\n",
|
||||
" 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",
|
||||
"\n",
|
||||
" if max(abs(Hy)) < eps:\n",
|
||||
" converged = True\n",
|
||||
" break\n",
|
||||
"\n",
|
||||
" 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",
|
||||
"\n",
|
||||
"pos, converted = lop_ils(rz, transmitter_pos, (900,90), hx=hx_ils)\n",
|
||||
"print('Iterated solution: ', pos)\n"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"output_type": "stream",
|
||||
"stream": "stdout",
|
||||
"text": [
|
||||
"iteration: 0\n",
|
||||
"innovation [-148.512 28.6789 -148.5361]\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",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 2\n",
|
||||
"innovation [-0.0463 -0.001 -0.0463]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 3\n",
|
||||
"innovation [-0. -0. -0.]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"Iterated solution: [ 800. 200.]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 19
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"So let's think about this. The first iteration is essentially performing the computation that the linear Kalman filter computes during the update step:\n",
|
||||
"\n",
|
||||
"$$\\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?"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"pos, converted = lop_ils(rz, transmitter_pos, (801, 201), hx=hx_ils)\n",
|
||||
"print('Iterated solution: ', pos)"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"output_type": "stream",
|
||||
"stream": "stdout",
|
||||
"text": [
|
||||
"iteration: 0\n",
|
||||
"innovation [-0.0009 -1.3868 -0.0024]\n",
|
||||
"pos [ 800.0014 199.9991]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 1\n",
|
||||
"innovation [-0.0016 -0. -0.0016]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 2\n",
|
||||
"innovation [-0. -0. -0.]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"Iterated solution: [ 800. 200.]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 20
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"The first iteration produced a better estimate, but it still could be improved upon by iterating.\n",
|
||||
"\n",
|
||||
"I injected no noise in the measurement to test and display the theoretical performance of the filter. Now let's see how it performs when we inject noise."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"# add some noise\n",
|
||||
"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",
|
||||
"print('Iterated solution: ', pos)"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"output_type": "stream",
|
||||
"stream": "stdout",
|
||||
"text": [
|
||||
"iteration: 0\n",
|
||||
"innovation [ 129.8823 100.461 107.5398]\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",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 2\n",
|
||||
"innovation [-0.6041 -0.3813 0.3569]\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",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 4\n",
|
||||
"innovation [-0.4802 0.0007 0.4803]\n",
|
||||
"pos [ 799.9476 198.6025]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"Iterated solution: [ 799.9475854 198.60245871]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 21
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"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. "
|
||||
]
|
||||
},
|
||||
{
|
||||
"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,
|
||||
|
||||
@@ -1847,480 +1847,9 @@
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"This is untenable behavior for a real world filter. `FilterPy`'s UKF code allows you to provide it a function to compute the residuals in cases of nonlinear behavior like this, but this requires more knowledge about `FilterPy`'s implementation than we yet process."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 2,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Iterative Least Squares for Sensor Fusion"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"** author's note: this topic is not specific to UKFs, and so it probably belongs elsewhere in the book. I'm not sure where I want to move it yet. Skip this if you are not interested in sensor fusion.**\n",
|
||||
"This is untenable behavior for a real world filter. `FilterPy`'s UKF code allows you to provide it a function to compute the residuals in cases of nonlinear behavior like this, but this requires more knowledge about `FilterPy`'s implementation than we yet process.\n",
|
||||
"\n",
|
||||
"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",
|
||||
"\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."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"with book_format.figsize(10,6):\n",
|
||||
" ukf_internal.show_three_gps()"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"metadata": {},
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAp8AAAGNCAYAAABXHhoFAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x7fba8c3cdb70>"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 25
|
||||
},
|
||||
{
|
||||
"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",
|
||||
"\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",
|
||||
"\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",
|
||||
"\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",
|
||||
"$$\\delta \\mathbf{z}^+ = \\mathbf{z} - h(\\mathbf{x}^+)$$\n",
|
||||
"\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",
|
||||
"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"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"$$ \\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",
|
||||
"\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",
|
||||
"\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",
|
||||
"\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",
|
||||
"$$\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",
|
||||
"$$\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."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 3,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Derivation of ILS Equations (Optional)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"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",
|
||||
"\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",
|
||||
"which expands to\n",
|
||||
"\n",
|
||||
"$$ \\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$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"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}$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Therefore the third term can be computed as\n",
|
||||
"\n",
|
||||
"$$\\frac{\\partial}{\\partial \\mathbf{x}}{\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H}\\delta \\mathbf{x} = {\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H}$$\n",
|
||||
"\n",
|
||||
"and the second term as\n",
|
||||
"\n",
|
||||
"$$\\frac{\\partial}{\\partial \\mathbf{x}}{\\delta \\mathbf{x}}^\\mathsf{T}\\mathbf{H}^\\mathsf{T}\\delta \\mathbf{z}^-={\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H}$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"We also know that\n",
|
||||
"$$\\frac{\\partial \\mathbf{B}^\\mathsf{T}\\mathbf{AB}}{\\partial \\mathbf{B}} = \\mathbf{B}^\\mathsf{T}(\\mathbf{A} + \\mathbf{A}^\\mathsf{T})$$\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"Therefore the first term becomes\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\begin{aligned}\n",
|
||||
"\\frac{\\partial}{\\partial \\mathbf{x}}{\\delta \\mathbf{x}}^\\mathsf{T}\\mathbf{H}^\\mathsf{T}\\mathbf{H}\\delta \\mathbf{x} &= {\\delta \\mathbf{x}}^\\mathsf{T}(\\mathbf{H}^\\mathsf{T}\\mathbf{H} + {\\mathbf{H}^\\mathsf{T}\\mathbf{H}}^\\mathsf{T})\\\\\n",
|
||||
"&= {\\delta \\mathbf{x}}^\\mathsf{T}(\\mathbf{H}^\\mathsf{T}\\mathbf{H} + \\mathbf{H}^\\mathsf{T}\\mathbf{H}) \\\\\n",
|
||||
"&= 2{\\delta \\mathbf{x}}^\\mathsf{T}\\mathbf{H}^\\mathsf{T}\\mathbf{H}\n",
|
||||
"\\end{aligned}$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Finally, the fourth term is\n",
|
||||
"\n",
|
||||
"$$ \\frac{\\partial}{\\partial \\mathbf{x}}\n",
|
||||
"{\\delta \\mathbf{z}^-}^\\mathsf{T}\\delta \\mathbf{z}^-=0$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Replacing the terms in the expanded partial differential equation gives us\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
" 2{\\delta \\mathbf{x}}^\\mathsf{T}\\mathbf{H}^\\mathsf{T}\\mathbf{H} -\n",
|
||||
" {\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H} - {\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H}\n",
|
||||
" =0\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"$${\\delta \\mathbf{x}}^\\mathsf{T}\\mathbf{H}^\\mathsf{T}\\mathbf{H} -\n",
|
||||
" {\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H} = 0$$\n",
|
||||
" \n",
|
||||
" $${\\delta \\mathbf{x}}^\\mathsf{T}\\mathbf{H}^\\mathsf{T}\\mathbf{H} =\n",
|
||||
" {\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H}$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Multiplying each side by $(\\mathbf{H}^\\mathsf{T}\\mathbf{H})^{-1}$ yields\n",
|
||||
"\n",
|
||||
"$${\\delta \\mathbf{x}}^\\mathsf{T} =\n",
|
||||
"{\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H}(\\mathbf{H}^\\mathsf{T}\\mathbf{H})^{-1}$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Taking the transpose of each side gives\n",
|
||||
"\n",
|
||||
"$${\\delta \\mathbf{x}} = ({{\\delta \\mathbf{z}^-}^\\mathsf{T}\\mathbf{H}(\\mathbf{H}^\\mathsf{T}\\mathbf{H})^{-1}})^\\mathsf{T} \\\\\n",
|
||||
"={{(\\mathbf{H}^\\mathsf{T}\\mathbf{H})^{-1}}^T\\mathbf{H}^\\mathsf{T} \\delta \\mathbf{z}^-} \\\\\n",
|
||||
"={{(\\mathbf{H}^\\mathsf{T}\\mathbf{H})^{-1}}\\mathbf{H}^\\mathsf{T} \\delta \\mathbf{z}^-}\n",
|
||||
"$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"For various reasons you may want to weigh some measurement more 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",
|
||||
"\n",
|
||||
"Replicating the math above with the added $\\mathbf{W}$ term results in\n",
|
||||
"\n",
|
||||
"$${\\delta \\mathbf{x}} = {{(\\mathbf{H}^\\mathsf{T}\\mathbf{WH})^{-1}}\\mathbf{H}^\\mathsf{T}\\mathbf{W} \\delta \\mathbf{z}^-}\n",
|
||||
"$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 3,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Implementing Iterative Least Squares"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"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",
|
||||
"\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",
|
||||
"from numpy import dot\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"np.random.seed(1234)\n",
|
||||
"user_pos = np.array([800, 200])\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"transmitter_pos = np.asarray(\n",
|
||||
" [[0, 1000],\n",
|
||||
" [0, -1000],\n",
|
||||
" [500, 500]], dtype=float)\n",
|
||||
"\n",
|
||||
"def transmitter_range(pos, transmitter_pos):\n",
|
||||
" \"\"\" Compute distance between position 'pos' and the list of positions\n",
|
||||
" in transmitter_pos\"\"\"\n",
|
||||
"\n",
|
||||
" N = len(transmitter_pos)\n",
|
||||
" rng = np.zeros(N)\n",
|
||||
"\n",
|
||||
" diff = np.asarray(pos) - transmitter_pos\n",
|
||||
"\n",
|
||||
" for i in range(N):\n",
|
||||
" rng[i] = norm(diff[i])\n",
|
||||
"\n",
|
||||
" return norm(diff, axis=1)\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"def hx_ils(pos, t_pos, r_est):\n",
|
||||
" N = len(t_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",
|
||||
" return H\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"def lop_ils(zs, t_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",
|
||||
"\n",
|
||||
" 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",
|
||||
" print('iteration:', i)\n",
|
||||
"\n",
|
||||
" H=hx(pos, t_pos, r_est) \n",
|
||||
" Hinv = inv(dot(H.T, H)).dot(H.T)\n",
|
||||
"\n",
|
||||
" #update position estimate\n",
|
||||
" y = zs - r_est\n",
|
||||
" 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",
|
||||
"\n",
|
||||
" if max(abs(Hy)) < eps:\n",
|
||||
" converged = True\n",
|
||||
" break\n",
|
||||
"\n",
|
||||
" 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",
|
||||
"\n",
|
||||
"pos, converted = lop_ils(rz, transmitter_pos, (900,90), hx=hx_ils)\n",
|
||||
"print('Iterated solution: ', pos)\n"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"output_type": "stream",
|
||||
"stream": "stdout",
|
||||
"text": [
|
||||
"iteration: 0\n",
|
||||
"innovation [-148.512 28.6789 -148.5361]\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",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 2\n",
|
||||
"innovation [-0.0463 -0.001 -0.0463]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 3\n",
|
||||
"innovation [-0. -0. -0.]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"Iterated solution: [ 800. 200.]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 26
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"So let's think about this. The first iteration is essentially performing the computation that the linear Kalman filter computes during the update step:\n",
|
||||
"\n",
|
||||
"$$\\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?"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"pos, converted = lop_ils(rz, transmitter_pos, (801, 201), hx=hx_ils)\n",
|
||||
"print('Iterated solution: ', pos)"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"output_type": "stream",
|
||||
"stream": "stdout",
|
||||
"text": [
|
||||
"iteration: 0\n",
|
||||
"innovation [-0.0009 -1.3868 -0.0024]\n",
|
||||
"pos [ 800.0014 199.9991]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 1\n",
|
||||
"innovation [-0.0016 -0. -0.0016]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 2\n",
|
||||
"innovation [-0. -0. -0.]\n",
|
||||
"pos [ 800. 200.]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"Iterated solution: [ 800. 200.]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 27
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"The first iteration produced a better estimate, but it still could be improved upon by iterating.\n",
|
||||
"\n",
|
||||
"I injected no noise in the measurement to test and display the theoretical performance of the filter. Now let's see how it performs when we inject noise."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"# add some noise\n",
|
||||
"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",
|
||||
"print('Iterated solution: ', pos)"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"output_type": "stream",
|
||||
"stream": "stdout",
|
||||
"text": [
|
||||
"iteration: 0\n",
|
||||
"innovation [ 129.8823 100.461 107.5398]\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",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 2\n",
|
||||
"innovation [-0.6041 -0.3813 0.3569]\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",
|
||||
"\n",
|
||||
"\n",
|
||||
"iteration: 4\n",
|
||||
"innovation [-0.4802 0.0007 0.4803]\n",
|
||||
"pos [ 799.9476 198.6025]\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"Iterated solution: [ 799.9475854 198.60245871]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 28
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"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. "
|
||||
"Finally, let's discuss the sensfor fusion method that we used. We used the measurement from each bearing and had the Kalman filter's equations compute the world position from the measurements. This is equivalent to doing a weighted least squares solution. In the *Kalman Filter Math* chapter we discussed the limited accuracy of such a scheme and introduced an *Iterative Least Squares* (ILS) algorithm to produce greater accuracy. If you wanted to use that scheme you would write an ILS algorithm to solve the geometry of your problem, and pass the result of that calculation into the `update()` method as the measurement to be filtered. This imposes on you the need to compute the correct noise matrix for this computed positions, which may not be trivial. Perhaps in a later release of this book I will develop an example, but regardless it will take you a significant amount of research and experiment to design the best method for your application. For example, the ILS is probably the most common algorithm used to turn GPS pseudoranges into a position, but the literature discusses a number of alternatives. Sometimes there are faster methods, sometimes the iterative method does not converge, or does not converge fast enough, or requires too much computation for an embedded system. "
|
||||
]
|
||||
},
|
||||
{
|
||||
|
||||
Reference in new issue
Block a user