From 510e720559c8fed816ef2b37a7abb39ae7e5832d Mon Sep 17 00:00:00 2001 From: Roger Labbe Date: Thu, 19 May 2016 18:39:26 -0700 Subject: [PATCH] I used terms better suited to radars (elevation angle instead of bearing, for example). --- 10-Unscented-Kalman-Filter.ipynb | 150 ++++++++++++++++--------------- 11-Extended-Kalman-Filters.ipynb | 42 ++++----- code/ekf_internal.py | 2 +- 3 files changed, 98 insertions(+), 96 deletions(-) diff --git a/10-Unscented-Kalman-Filter.ipynb b/10-Unscented-Kalman-Filter.ipynb index f9df9e3..2f2a8b4 100644 --- a/10-Unscented-Kalman-Filter.ipynb +++ b/10-Unscented-Kalman-Filter.ipynb @@ -1090,7 +1090,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -1917,7 +1917,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -2732,7 +2732,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -2745,7 +2745,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "Difference in mean x=-0.080, y=43.686\n" + "Difference in mean x=-0.023, y=43.732\n" ] } ], @@ -3569,7 +3569,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -4387,7 +4387,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -5201,7 +5201,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -6017,7 +6017,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -6984,7 +6984,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -7872,7 +7872,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -8747,7 +8747,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -8806,9 +8806,9 @@ "\n", "Let's tackle our first nonlinear problem. We will write a filter to track an airplane using radar as the sensor. To keep the problem similar to the previous one as possible we will track in two dimensions. We will track one dimension on the ground and the altitude of the aircraft. Each dimension is independent so we can do this with no loss of generality.\n", "\n", - "Radars work by emitting radio waves or microwaves. Anything in the beam's path will reflect some of the signal back to the radar. By timing how long it takes for the reflected signal to return it can compute the *slant distance* and bearing to the target. Slant distance is the straight line distance from the radar to the object. Radars also provide velocity measurements via the Doppler effect, but we will be ignoring this complication for the moment.\n", + "Radars work by emitting radio waves or microwaves. Anything in the beam's path will reflect some of the signal back to the radar. By timing how long it takes for the reflected signal to return it can compute the *slant distance* to the target. Slant distance is the straight line distance from the radar to the object. Bearing is computed using the *directive gain* of the antenna.\n", "\n", - "We compute the (x,y) position of the aircraft from the slant distance and bearing as illustrated by this diagram:" + "We compute the (x,y) position of the aircraft from the slant distance and elevation angle as illustrated by this diagram:" ] }, { @@ -9586,7 +9586,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -9599,13 +9599,15 @@ "source": [ "import code.ekf_internal as ekf_internal\n", "with interactive_plot():\n", - " ekf_internal.show_radar_chart()" + " ekf_internal.show_radar_chart();" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ + "The *elevation angle* $\\epsilon$ is the angle above the line of sight formed by the ground.\n", + "\n", "We will assume that the aircraft is flying at a constant altitude. Thus we have a three variable state vector:\n", "\n", "$$\\mathbf x = \\begin{bmatrix}\\mathtt{distance} \\\\\\mathtt{velocity}\\\\ \\mathtt{altitude}\\end{bmatrix}= \\begin{bmatrix}x \\\\ \\dot x\\\\ y\\end{bmatrix}$$" @@ -9647,15 +9649,15 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "Next we design the measurement function. As in the linear Kalman filter the measurement function converts the filter's prior into a measurement. We need to convert the position and velocity of the aircraft into the bearing and range from the radar station.\n", + "Next we design the measurement function. As in the linear Kalman filter the measurement function converts the filter's prior into a measurement. We need to convert the position and velocity of the aircraft into the elevation angle and range from the radar station.\n", "\n", "Range is computed with the Pythagorean theorem:\n", "\n", "$$\\text{range} = \\sqrt{(x_\\text{ac} - x_\\text{radar})^2 + (y_\\text{ac} - y_\\mathtt{radar})^2}$$\n", "\n", - "Bearing is the arctangent of $y/x$:\n", + "The elevation angle $\\epsilon$ is the arctangent of $y/x$:\n", "\n", - "$$\\text{bearing} = \\tan^{-1}{\\frac{y_\\mathtt{ac} - y_\\text{radar}}{x_\\text{ac} - x_\\text{radar}}}$$\n", + "$$\\epsilon = \\tan^{-1}{\\frac{y_\\mathtt{ac} - y_\\text{radar}}{x_\\text{ac} - x_\\text{radar}}}$$\n", "\n", "We need to define a Python function to compute this. I'll take advantage of the fact that a function can own a variable to store the radar's position. While this isn't necessary for this problem (we could hard code the value, or use a global), this gives the function more flexibility." ] @@ -9671,10 +9673,10 @@ "source": [ "def h_radar(x):\n", " dx = x[0] - h_radar.radar_pos[0]\n", - " dz = x[2] - h_radar.radar_pos[1]\n", - " slant_range = math.sqrt(dx**2 + dz**2)\n", - " bearing = math.atan2(dz, dx)\n", - " return slant_range, bearing\n", + " dy = x[2] - h_radar.radar_pos[1]\n", + " slant_range = math.sqrt(dx**2 + dy**2)\n", + " elevation_angle = math.atan2(dy, dx)\n", + " return slant_range, elevation_angle\n", "\n", "h_radar.radar_pos = (0, 0)" ] @@ -9702,15 +9704,15 @@ "\n", "class RadarStation(object):\n", " \n", - " def __init__(self, pos, range_std, bearing_std):\n", + " def __init__(self, pos, range_std, elev_angle_std):\n", " self.pos = np.asarray(pos) \n", " self.range_std = range_std\n", - " self.bearing_std = bearing_std\n", + " self.elev_angle_std = elev_angle_std\n", "\n", " \n", " def reading_of(self, ac_pos):\n", - " \"\"\" Returns (range, bearing) to aircraft. \n", - " Bearing is in radians.\n", + " \"\"\" Returns (range, elevation angle) to aircraft. \n", + " Elevation angle is in radians.\n", " \"\"\"\n", " \n", " diff = np.subtract(ac_pos, self.pos)\n", @@ -9720,12 +9722,12 @@ "\n", "\n", " def noisy_reading(self, ac_pos):\n", - " \"\"\" Compute range and bearing to aircraft with \n", + " \"\"\" Compute range and elevation angle to aircraft with \n", " simulated noise\"\"\"\n", " \n", " rng, brg = self.reading_of(ac_pos) \n", " rng += randn() * self.range_std\n", - " brg += randn() * self.bearing_std \n", + " brg += randn() * self.elev_angle_std \n", " return rng, brg \n", "\n", "class ACSim(object): \n", @@ -9747,13 +9749,13 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "A military grade radar achieves 1 meter RMS range accuracy, and 1 mrad RMS for bearing [1]. We will assume a more modest 5 meter range accuracy, and 0.5° angular accuracy as this provides a more challenging data set for the filter.\n", + "A military grade radar achieves 1 meter RMS range accuracy, and 1 mrad RMS for elevation angle [1]. We will assume a more modest 5 meter range accuracy, and 0.5° angular accuracy as this provides a more challenging data set for the filter.\n", "\n", "The design of $\\mathbf Q$ requires some discussion. The state is $\\begin{bmatrix}x & \\dot x & y\\end{bmatrix}^\\mathtt{T}$. The first two elements are down range distance and velocity, so we can use `Q_discrete_white_noise` noise to compute the values for the upper left hand side of Q. The third element is altitude, which we assume is independent of $x$. That results in a block design for $\\mathbf Q$:\n", "\n", "$$\\mathbf Q = \\begin{bmatrix}\\mathbf Q_\\mathtt{x} & \\boldsymbol 0 \\\\ \\boldsymbol 0 & Q_\\mathtt{y}\\end{bmatrix}$$\n", "\n", - "I'll start with the aircraft positioned directly over the radar station, flying at 100 m/s. A typical radar might update only once every 12 seconds so we will use that for our epoch period. " + "I'll start with the aircraft positioned directly over the radar station, flying at 100 m/s. A typical height finder radar might update only once every 3 seconds so we will use that for our epoch period. " ] }, { @@ -10530,7 +10532,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -11306,7 +11308,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -12082,7 +12084,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -12097,9 +12099,9 @@ "import math\n", "from code.ukf_internal import plot_radar\n", "\n", - "dt = 12. # 12 seconds between readings\n", + "dt = 3. # 12 seconds between readings\n", "range_std = 5 # meters\n", - "bearing_std = math.radians(0.5)\n", + "elevation_angle_std = math.radians(0.5)\n", "ac_pos = (0., 1000.)\n", "ac_vel = (100., 0.)\n", "radar_pos = (0., 0.)\n", @@ -12111,13 +12113,13 @@ "kf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1)\n", "kf.Q[2,2] = 0.1\n", "\n", - "kf.R = np.diag([range_std**2, bearing_std**2])\n", + "kf.R = np.diag([range_std**2, elevation_angle_std**2])\n", "kf.x = np.array([0., 90., 1100.])\n", "kf.P = np.diag([300**2, 30**2, 150**2])\n", "\n", "random.seed(200)\n", "pos = (0, 0)\n", - "radar = RadarStation(pos, range_std, bearing_std)\n", + "radar = RadarStation(pos, range_std, elevation_angle_std)\n", "ac = ACSim(ac_pos, (100, 0), 0.02)\n", "\n", "time = np.arange(0, 360 + dt, dt)\n", @@ -12927,7 +12929,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -12940,8 +12942,8 @@ "name": "stdout", "output_type": "stream", "text": [ - "Actual altitude: 2561.9\n", - "UKF altitude : 1107.2\n" + "Actual altitude: 2515.6\n", + "UKF altitude : 1042.1\n" ] } ], @@ -13797,7 +13799,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -13810,8 +13812,8 @@ "name": "stdout", "output_type": "stream", "text": [ - "Actual altitude: 2561.9\n", - "UKF altitude : 2432.9\n" + "Actual altitude: 2515.6\n", + "UKF altitude : 2500.1\n" ] } ], @@ -13819,7 +13821,7 @@ "random.seed(200)\n", "ac = ACSim(ac_pos, (100, 0), 0.02)\n", "\n", - "kf_cv = cv_UKF(f_cv_radar, h_radar, R_std=[range_std, bearing_std])\n", + "kf_cv = cv_UKF(f_cv_radar, h_radar, R_std=[range_std, elevation_angle_std])\n", "time = np.arange(0, 360 + dt, dt)\n", "xs, ys = [], []\n", "for t in time:\n", @@ -14638,7 +14640,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -14657,13 +14659,13 @@ ], "source": [ "range_std = 500.\n", - "bearing_std = math.degrees(0.5)\n", + "elevation_angle_std = math.degrees(0.5)\n", "random.seed(200)\n", "pos = (0, 0)\n", - "radar = RadarStation(pos, range_std, bearing_std)\n", + "radar = RadarStation(pos, range_std, elevation_angle_std)\n", "ac = ACSim(ac_pos, (100, 0), 0.02)\n", "\n", - "kf_sf = cv_UKF(f_cv_radar, h_radar, R_std=[range_std, bearing_std])\n", + "kf_sf = cv_UKF(f_cv_radar, h_radar, R_std=[range_std, elevation_angle_std])\n", "time = np.arange(0, 360 + dt, dt)\n", "xs = []\n", "for _ in time:\n", @@ -14682,11 +14684,11 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "For Doppler we need to include the velocity in $x$ and $y$ into the measurement. The `ACSim` class stores velocity in the data member `vel`. To perform the Kalman filter update we just need to call `update` with a list containing the slant distance, bearing, and velocity in $x$ and $y$:\n", + "For Doppler we need to include the velocity in $x$ and $y$ into the measurement. The `ACSim` class stores velocity in the data member `vel`. To perform the Kalman filter update we just need to call `update` with a list containing the slant distance, elevation angle, and velocity in $x$ and $y$:\n", "\n", - "$$z = [\\mathtt{slant\\_range},\\, \\text{bearing},\\, \\dot x,\\, \\dot y]$$\n", + "$$z = [\\mathtt{slant\\_range},\\, \\text{elevation angle},\\, \\dot x,\\, \\dot y]$$\n", "\n", - "The measurement contains four values so the measurement function also needs to return four values. The slant range and bearing will be computed as before, and we do not need to compute the velocity in $x$ and $y$ as they are provided by the state estimate." + "The measurement contains four values so the measurement function also needs to return four values. The slant range and elevation angle will be computed as before, and we do not need to compute the velocity in $x$ and $y$ as they are provided by the state estimate." ] }, { @@ -14701,8 +14703,8 @@ " dx = x[0] - h_vel.radar_pos[0]\n", " dz = x[2] - h_vel.radar_pos[1]\n", " slant_range = math.sqrt(dx**2 + dz**2)\n", - " bearing = math.atan2(dz, dx)\n", - " return slant_range, bearing, x[1], x[3]" + " elevation_angle = math.atan2(dz, dx)\n", + " return slant_range, elevation_angle, x[1], x[3]" ] }, { @@ -15487,7 +15489,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -15500,7 +15502,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "Velocity std 1.3 m/s\n" + "Velocity std 0.9 m/s\n" ] } ], @@ -15509,15 +15511,15 @@ "h_vel.radar_pos = (0, 0)\n", "\n", "range_std = 500.\n", - "bearing_std = math.degrees(0.5)\n", + "elevation_angle_std = math.degrees(0.5)\n", "vel_std = 2.\n", "\n", "random.seed(200)\n", "ac = ACSim(ac_pos, (100, 0), 0.02)\n", - "radar = RadarStation((0, 0), range_std, bearing_std)\n", + "radar = RadarStation((0, 0), range_std, elevation_angle_std)\n", "\n", "kf_sf2 = cv_UKF(f_cv_radar, h_vel, \n", - " R_std=[range_std, bearing_std, vel_std, vel_std])\n", + " R_std=[range_std, elevation_angle_std, vel_std, vel_std])\n", "\n", "time = np.arange(0, 360 + dt, dt)\n", "xs = []\n", @@ -16330,7 +16332,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -17213,7 +17215,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -18052,7 +18054,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -18865,7 +18867,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -19685,7 +19687,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -20493,7 +20495,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -21668,7 +21670,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -22444,7 +22446,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -23220,7 +23222,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -24058,7 +24060,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -24834,7 +24836,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -25610,7 +25612,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -26577,7 +26579,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -27781,7 +27783,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -28637,7 +28639,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -29444,7 +29446,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" diff --git a/11-Extended-Kalman-Filters.ipynb b/11-Extended-Kalman-Filters.ipynb index e58fa6e..a36ff59 100644 --- a/11-Extended-Kalman-Filters.ipynb +++ b/11-Extended-Kalman-Filters.ipynb @@ -1112,7 +1112,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -1195,7 +1195,7 @@ "\n", "Radars work by emitting a beam of radio waves and scanning for a return bounce. Anything in the beam's path will reflects some of the signal back to the radar. By timing how long it takes for the reflected signal to get back to the radar the system can compute the *slant distance* - the straight line distance from the radar installation to the object.\n", "\n", - "The relationship between the radar's slant range distance and bearing with the horizontal position $x$ and altitude $y$ of the aircraft is illustrated in the figure below:" + "The relationship between the radar's slant range distance $r$ and elevation angle $\\epsilon$ with the horizontal position $x$ and altitude $y$ of the aircraft is illustrated in the figure below:" ] }, { @@ -1973,7 +1973,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -3052,7 +3052,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -3828,7 +3828,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -4604,7 +4604,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -5511,7 +5511,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -6925,7 +6925,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -6938,7 +6938,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "Final P: [ 0.0232 0.0439 0.00223]\n" + "Final P: [ 0.0246 0.0406 0.00218]\n" ] } ], @@ -7738,7 +7738,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -7751,7 +7751,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "Final P: [ 0.0195 0.0212 0.00155]\n" + "Final P: [ 0.0201 0.0204 0.00154]\n" ] } ], @@ -8547,7 +8547,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -8560,7 +8560,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "Final P: [ 0.0188 0.0457 0.000222]\n" + "Final P: [ 0.021 0.0446 0.000226]\n" ] } ], @@ -9353,7 +9353,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -9366,7 +9366,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "Final P: [ 0.336 0.796 0.00376]\n" + "Final P: [ 0.278 0.783 0.00357]\n" ] } ], @@ -10159,7 +10159,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -10172,7 +10172,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "Final P: [ 0.00853 0.00873 0.000763]\n" + "Final P: [ 0.00854 0.00864 0.000761]\n" ] } ], @@ -10989,7 +10989,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -11002,7 +11002,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "actual mean=1.30, std=1.12\n", + "actual mean=1.30, std=1.13\n", "EKF mean=1.00, std=0.95\n" ] } @@ -11795,7 +11795,7 @@ { "data": { "text/html": [ - "" + "" ], "text/plain": [ "" @@ -11808,7 +11808,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "actual mean=1.30, std=1.13\n", + "actual mean=1.30, std=1.14\n", "UKF mean=1.30, std=1.08\n" ] } diff --git a/code/ekf_internal.py b/code/ekf_internal.py index 3ab565b..a6ee0b5 100644 --- a/code/ekf_internal.py +++ b/code/ekf_internal.py @@ -215,7 +215,7 @@ def show_radar_chart(): - ax.annotate('$\Theta$', xy=(1.2, 1.05), color='b') + ax.annotate('$\epsilon$', xy=(1.2, 1.05), color='b') ax.annotate('Aircraft', xy=(2.04,2.), color='b') ax.annotate('altitude (y)', xy=(2.04,1.5), color='k') ax.annotate('x', xy=(1.5, .9))