Copy edit of UKF chapter.
Also found a mistake in the text about computing the residual of angles that was duplicated in the EKF chapter, so I fixed the error in that chapter as well. I pulled out the 'track in a circle' example as it is the usual sort of textbook nonsense problem that no one cares about. I created an 'old-content' Notebook in case anyone still wants to see it; I may put more content there as I continue to edit.
This commit is contained in:
Roger Labbe
parent
6092e06459
commit
43070a4439
File diff suppressed because one or more lines are too long
@ -1536,7 +1536,7 @@
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Now we have another issue to handle. The residual is notionally computed as $y = z - h(x)$ but this will not work because our measurement contains an angle in it. Suppose z has a bearing of $1^\\circ$ and $h(x)$ has a bearing of $359^\\circ$. Naively subtracting them would yield a bearing difference of $-358^\\circ$, which will throw off the computation of the Kalman gain. The correct angle difference in this case is $-2^\\circ$. So we will have to write code to correctly compute the bearing residual."
|
||||
"Now we have another issue to handle. The residual is notionally computed as $y = z - h(x)$ but this will not work because our measurement contains an angle in it. Suppose z has a bearing of $1^\\circ$ and $h(x)$ has a bearing of $359^\\circ$. Naively subtracting them would yield a bearing difference of $-358^\\circ$, which will throw off the computation of the Kalman gain. The correct angle difference in this case is $2^\\circ$. So we will have to write code to correctly compute the bearing residual."
|
||||
]
|
||||
},
|
||||
{
|
||||
@ -1547,7 +1547,7 @@
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"def residual(a,b):\n",
|
||||
"def residual(a, b):\n",
|
||||
" \"\"\" compute residual (a-b) between two measurement containing \n",
|
||||
" [range, bearing]. Bearing is normalized to [-pi, pi)\"\"\"\n",
|
||||
" y = a - b\n",
|
||||
|
||||
@ -204,7 +204,7 @@ def show_radar_chart():
|
||||
|
||||
|
||||
|
||||
ax.annotate('$\Theta$ (', xy=(1.2, 1.1), color='b')
|
||||
ax.annotate('$\Theta$', xy=(1.2, 1.1), color='b')
|
||||
ax.annotate('Aircraft', xy=(2.04,2.), color='b')
|
||||
ax.annotate('altitude', xy=(2.04,1.5), color='k')
|
||||
ax.annotate('X', xy=(1.5, .9))
|
||||
|
||||
@ -381,7 +381,7 @@ def _plot_iscts(pos, sa, sb, N=4):
|
||||
plt.scatter(xs_a, ys_a)
|
||||
plt.scatter(xs_b, ys_b)
|
||||
plt.gca().set_aspect('equal')
|
||||
plt.show()
|
||||
|
||||
|
||||
def plot_iscts_two_sensors():
|
||||
pos = np.array([4., 4,])
|
||||
@ -401,6 +401,7 @@ def plot_iscts_two_sensors_changed_sensors():
|
||||
plt.scatter(*sa, s=100)
|
||||
plt.scatter(*sb, s=100)
|
||||
_plot_iscts(pos, sa, sb, N=5)
|
||||
plt.ylim(3.8, 8.5)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
275
old-content.ipynb
Normal file
275
old-content.ipynb
Normal file
@ -0,0 +1,275 @@
|
||||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"This contains text and code that I've removed from the book because it is redundant or replaced with what I think of as better material. Maybe you liked it, so I'll save it here for now."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Exercise: Track a target moving in a circle"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Change the simulated target movement to move in a circle. Avoid angular nonlinearities by putting the sensors well outside the movement range of the target, and avoid the angles $0^\\circ$ and $180^\\circ$."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {
|
||||
"collapsed": true
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"# your solution here"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Solution\n",
|
||||
"We have a few choices here. First, if we know the movement of the target we can design our filter's state transition function so that it correctly predicts the circular movement. For example, suppose we were tracking a boat race optically - we would want to take track shape into account with our filter. However, in this chapter we have not been talking about such constrained behavior, so I will not build knowledge of the movement into the filter. So my implementation looks like this."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"metadata": {
|
||||
"collapsed": true
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"def plot_circular_target(kf, std_noise, target_pos):\n",
|
||||
" xs = []\n",
|
||||
" txs = []\n",
|
||||
" radius = 100\n",
|
||||
" for i in range(300):\n",
|
||||
" target_pos[0] = math.cos(i/10)*radius + randn()*0.0001\n",
|
||||
" target_pos[1] = math.sin(i/10)*radius + randn()*0.0001\n",
|
||||
" txs.append((target_pos[0], target_pos[1]))\n",
|
||||
"\n",
|
||||
" z = measurement(sa_pos, sb_pos, target_pos)\n",
|
||||
" z[0] += randn() * std_noise\n",
|
||||
" z[1] += randn() * std_noise\n",
|
||||
"\n",
|
||||
" kf.predict()\n",
|
||||
" kf.update(z)\n",
|
||||
" xs.append(kf.x)\n",
|
||||
"\n",
|
||||
" xs = np.asarray(xs)\n",
|
||||
" txs = np.asarray(txs)\n",
|
||||
"\n",
|
||||
" plt.plot(xs[:, 0], xs[:, 2])\n",
|
||||
" plt.plot(txs[: ,0], txs[:, 1], linestyle='-.')\n",
|
||||
" plt.axis('equal')\n",
|
||||
" plt.show()\n",
|
||||
"\n",
|
||||
"sa_pos = [-240, 200]\n",
|
||||
"sb_pos = [240, 200]"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {
|
||||
"collapsed": true
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"np.random.seed(12283)\n",
|
||||
"std_noise = math.radians(0.5)\n",
|
||||
"target_pos = [0, 0]\n",
|
||||
"f = moving_target_filter(target_pos, std_noise, Q=1.1)\n",
|
||||
"plot_circular_target(f, std_noise, target_pos)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Discussion\n",
|
||||
"\n",
|
||||
"The filter tracks the movement of the target, but never really converges on the track. This is because the filter is modeling a constant velocity target, but the target is anything but constant velocity. As mentioned above we could model the circular behavior by defining the `fx()` function, but then we would have problems when the target is not moving in a circle. Instead, lets tell the filter we are are less sure about our process model by making $\\mathbf{Q}$ larger. Here I have increased the variance from 0.1 to 1.0"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {
|
||||
"collapsed": true
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"np.random.seed(12283)\n",
|
||||
"std_noise = math.radians(0.5)\n",
|
||||
"cf = moving_target_filter(target_pos, std_noise, Q=10.)\n",
|
||||
"target_pos = [0, 0]\n",
|
||||
"plot_circular_target(cf, std_noise, target_pos)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"The convergence is not perfect, but it is far better. "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Exercise: Sensor Position Effects\n",
|
||||
"\n",
|
||||
"Is the behavior of the filter invariant for any sensor position? Find a sensor position that produces bad filter behavior."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {
|
||||
"collapsed": true
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"# your answer here"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Solution\n",
|
||||
"\n",
|
||||
"We have already discussed the problem of angles being modal, so causing that problem by putting the sensors at `y=0` would be a trivial solution. However, let's be more subtle than that. We want to create a situation where there are an infinite number of solutions for the sensor readings. We can achieve that whenever the target lies on the straight line between the two sensors. In that case there is no triangulation possible and there is no unique solution. My solution is as follows."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {
|
||||
"collapsed": true
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"std_noise = math.radians(0.5)\n",
|
||||
"sa_pos = [-200, 200]\n",
|
||||
"sb_pos = [200, -200]\n",
|
||||
"plt.scatter(*sa_pos, s=200)\n",
|
||||
"plt.scatter(*sb_pos, s=200)\n",
|
||||
"target_pos = [0, 0]\n",
|
||||
"cf = moving_target_filter(target_pos, std_noise, Q=10.)\n",
|
||||
"plot_circular_target(cf, std_noise, target_pos)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"I put the sensors at the upper left hand side and lower right hand side of the target's movement. We can see how the filter diverges when the target enters the region directly between the two sensors. The state transition always predicts that the target is moving in a straight line. When the target is between the two sensors this straight line movement is well described the bearing measurements from the two sensors so the filter estimate starts to approximate a straight line. "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Exercise: Compute Position Errors\n",
|
||||
"\n",
|
||||
"The position errors of the filter vary depending on how far away the target is from a sensor. Write a function that computes the distance error due to a bearing error. "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"metadata": {
|
||||
"collapsed": true
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"# your solution here"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Solution\n",
|
||||
"\n",
|
||||
"Basic trigonometry gives us this answer."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {
|
||||
"collapsed": true
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"def distance_error(target_distance, angle_error):\n",
|
||||
" x = 1 - math.cos(angle_error)\n",
|
||||
" y = math.sin(angle_error)\n",
|
||||
" return target_distance*(x**2 + y**2)**.5\n",
|
||||
"\n",
|
||||
"d = distance_error(100, math.radians(1.))\n",
|
||||
"print('\\n\\nError of 1 degree at 100km is {:.3}km'.format(d))"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {
|
||||
"collapsed": true
|
||||
},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {
|
||||
"collapsed": true
|
||||
},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {
|
||||
"collapsed": true
|
||||
},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.4.1"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 0
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user