Removing flab from language.
This commit is contained in:
Roger Labbe
parent
fb7db6d752
commit
ffaee9b080
@ -290,7 +290,7 @@
|
||||
"source": [
|
||||
"## Linearizing the Kalman Filter\n",
|
||||
"\n",
|
||||
"The Kalman filter uses linear equations, so it does not work with nonlinear problems. Problems can be nonlinear in two ways. First, the process model might be nonlinear. An object falling through the atmosphere encounters drag which reduces its acceleration. The amount of drag varies based on the velocity the object. The resulting behavior is nonlinear - it cannot be modeled with linear equations. Second, the measurements could be nonlinear. For example, a radar gives a range and bearing to a target. We use trigonometry, which is nonlinear, to compute the position of the target.\n",
|
||||
"The Kalman filter uses linear equations, so it does not work with nonlinear problems. Problems can be nonlinear in two ways. First, the process model might be nonlinear. An object falling through the atmosphere encounters drag which reduces its acceleration. The drag coefficient varies based on the velocity the object. The resulting behavior is nonlinear - it cannot be modeled with linear equations. Second, the measurements could be nonlinear. For example, a radar gives a range and bearing to a target. We use trigonometry, which is nonlinear, to compute the position of the target.\n",
|
||||
"\n",
|
||||
"For the linear filter we have these equations for the process and measurement models:\n",
|
||||
"\n",
|
||||
@ -342,18 +342,18 @@
|
||||
"source": [
|
||||
"If the curve above is the process model, then the dotted lines shows the linearization of that curve for the estimate $x=1.5$.\n",
|
||||
"\n",
|
||||
"We linearize systems by finding the slope of the curve at the given point:\n",
|
||||
"We linearize systems by taking the derivative, which finds an expression for the slope of the curve:\n",
|
||||
"\n",
|
||||
"$$\\begin{aligned}\n",
|
||||
"f(x) &= x^2 -2x \\\\\n",
|
||||
"\\frac{df}{dx} &= 2x - 2\n",
|
||||
"\\end{aligned}$$\n",
|
||||
"\n",
|
||||
"and then finding its value at the evaluation point:\n",
|
||||
"and then evaluating it at $x$:\n",
|
||||
"\n",
|
||||
"$$\\begin{aligned}m &= f'(x=1.5) \\\\&= 2(1.5) - 2 \\\\&= 1\\end{aligned}$$ \n",
|
||||
"\n",
|
||||
"Our math will be more complicated because we are working with systems of differential equations. We linearize $f(\\mathbf x, \\mathbf u)$, and $h(\\mathbf x)$ by taking the partial derivatives ($\\frac{\\partial}{\\partial \\mathbf x}$) of each to evaluate $\\mathbf A$ and $\\mathbf H$ at the point $\\mathbf x_t$ and $\\mathbf u_t$. This gives us the the system dynamics matrix and measurement model matrix:\n",
|
||||
"Linearizing systems of differential equations is more complicated. We linearize $f(\\mathbf x, \\mathbf u)$, and $h(\\mathbf x)$ by taking the partial derivatives of each to evaluate $\\mathbf A$ and $\\mathbf H$ at the point $\\mathbf x_t$ and $\\mathbf u_t$. We call the partial derivative of a matrix the *Jacobian*. This gives us the the system dynamics matrix and measurement model matrix:\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\begin{aligned}\n",
|
||||
@ -361,20 +361,22 @@
|
||||
"&= {\\frac{\\partial{f(\\mathbf x_t, \\mathbf u_t)}}{\\partial{\\mathbf x}}}\\biggr|_{{\\mathbf x_t},{\\mathbf u_t}} \\\\\n",
|
||||
"\\mathbf H &= \\frac{\\partial{h(\\mathbf x_t)}}{\\partial{\\mathbf x}}\\biggr|_{\\mathbf x_t} \n",
|
||||
"\\end{aligned}\n",
|
||||
"$$"
|
||||
"$$\n",
|
||||
"\n",
|
||||
"$h(\\overline{\\mathbf x})$ is computed with the prior, but I drop the bar on for notational convenience."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Finally, we find the discrete state transition matrix $\\mathbf F$ by using the approximation of the Taylor-series expansion of $e^{\\mathbf A \\Delta t}$:\n",
|
||||
"Finally, we find the discrete state transition matrix $\\mathbf F$ by using the Taylor series expansion of $e^{\\mathbf A \\Delta t}$:\n",
|
||||
"\n",
|
||||
"$$\\mathbf F = e^{\\mathbf A\\Delta t} = \\mathbf{I} + \\mathbf A\\Delta t + \\frac{(\\mathbf A\\Delta t)^2}{2!} + \\frac{(\\mathbf A\\Delta t)^3}{3!} + ... $$\n",
|
||||
"\n",
|
||||
"Alternatively, you can use one of the other techniques we learned in the **Kalman Math** chapter. \n",
|
||||
"\n",
|
||||
"This leads to the following equations for the EKF. I placed them beside the equations for the linear Kalman filter, and put boxes around the only changes:\n",
|
||||
"This leads to the following equations for the EKF. I placed them beside the equations for the linear Kalman filter, and put boxes around the changes:\n",
|
||||
"\n",
|
||||
"$$\\begin{array}{l|l}\n",
|
||||
"\\text{linear Kalman filter} & \\text{EKF} \\\\\n",
|
||||
@ -385,40 +387,41 @@
|
||||
"\\mathbf{\\overline P} = \\mathbf{FPF}^\\mathsf{T}+\\mathbf Q & \\mathbf{\\overline P} = \\mathbf{FPF}^\\mathsf{T}+\\mathbf Q \\\\\n",
|
||||
"\\hline\n",
|
||||
"& \\boxed{\\mathbf H = \\frac{\\partial{h(\\mathbf x_t)}}{\\partial{\\mathbf x}}\\biggr|_{\\mathbf x_t}} \\\\\n",
|
||||
"\\textbf{y} = \\mathbf z - \\mathbf{H \\bar{x}} & \\textbf{y} = \\mathbf z -\\mathbf{H \\bar{x}}\\\\\n",
|
||||
"\\textbf{y} = \\mathbf z - \\mathbf{H \\bar{x}} & \\textbf{y} = \\mathbf z - \\boxed{h(\\bar{x})}\\\\\n",
|
||||
"\\mathbf{K} = \\mathbf{\\bar{P}H}^\\mathsf{T} (\\mathbf{H\\bar{P}H}^\\mathsf{T} + \\mathbf R)^{-1} & \\mathbf{K} = \\mathbf{\\bar{P}H}^\\mathsf{T} (\\mathbf{H\\bar{P}H}^\\mathsf{T} + \\mathbf R)^{-1} \\\\\n",
|
||||
"\\mathbf x=\\mathbf{\\bar{x}} +\\mathbf{K\\textbf{y}} & \\mathbf x=\\mathbf{\\bar{x}} +\\mathbf{K\\textbf{y}} \\\\\n",
|
||||
"\\mathbf P= (\\mathbf{I}-\\mathbf{KH})\\mathbf{\\bar{P}} & \\mathbf P= (\\mathbf{I}-\\mathbf{KH})\\mathbf{\\bar{P}}\n",
|
||||
"\\end{array}$$\n",
|
||||
"\n",
|
||||
"We don't normally use $\\mathbf{Fx}$ to propagate the state for the EKF as the linearization causes inaccuracies. It is typical to compute $\\overline{\\mathbf x}$ using a numerical integration technique such as Euler or Runge Kutta. Thus I wrote $\\mathbf{\\overline x} = f(\\mathbf x, \\mathbf u)$.\n",
|
||||
"We don't normally use $\\mathbf{Fx}$ to propagate the state for the EKF as the linearization causes inaccuracies. It is typical to compute $\\overline{\\mathbf x}$ using a suitable numerical integration technique such as Euler or Runge Kutta. Thus I wrote $\\mathbf{\\overline x} = f(\\mathbf x, \\mathbf u)$. For the same reasons we don't use $\\mathbf{H\\overline{x}}$ in the computation for the residual, opting for the more accurate $h(\\overline{\\mathbf x})$.\n",
|
||||
"\n",
|
||||
"I think the easiest way to understand the EKF is to start off with an example. After we do a few examples you may want to come back and reread this section."
|
||||
"I think the easiest way to understand the EKF is to start off with an example. Later you may want to come back and reread this section."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Example: Tracking a Flying Airplane"
|
||||
"## Example: Tracking a Airplane"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"We will start by simulating tracking an airplane by using ground based radar. We implemented a UKF for this problem in the last chapter. Now we will implement an EKF for the same problem so we can compare both the filter performance and the level of effort required to implement the filter.\n",
|
||||
"This example tracks an airplane using ground based radar. We implemented a UKF for this problem in the last chapter. Now we will implement an EKF for the same problem so we can compare both the filter performance and the level of effort required to implement the filter.\n",
|
||||
"\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",
|
||||
"For this example we want to take the slant range measurement from the radar and compute the horizontal position (distance of aircraft from the radar measured over the ground) and altitude of the aircraft, as in the diagram below."
|
||||
"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:"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 3,
|
||||
"metadata": {
|
||||
"collapsed": false
|
||||
"collapsed": false,
|
||||
"scrolled": true
|
||||
},
|
||||
"outputs": [
|
||||
{
|
||||
@ -441,20 +444,18 @@
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"This gives us the equality $x=\\sqrt{slant^2 - altitude^2}$. "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Design the State Variables"
|
||||
"This gives us the equalities:\n",
|
||||
"\n",
|
||||
"$$\\theta = \\tan^{-1} \\frac y x\\\\\n",
|
||||
"r^2 = x^2 + y^2$$ "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Design the State Variables\n",
|
||||
"\n",
|
||||
"We want to track the position of an aircraft assuming a constant velocity and altitude, and measurements of the slant distance to the aircraft. That means we need 3 state variables - horizontal distance, horizonal velocity, and altitude:\n",
|
||||
"\n",
|
||||
"$$\\mathbf x = \\begin{bmatrix}\\mathtt{distance} \\\\\\mathtt{velocity}\\\\ \\mathtt{altitude}\\end{bmatrix}= \\begin{bmatrix}x \\\\ \\dot x\\\\ y\\end{bmatrix}$$"
|
||||
@ -464,13 +465,8 @@
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Design the Process Model"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Design the Process Model\n",
|
||||
"\n",
|
||||
"We assume a Newtonian, kinematic system for the aircraft. We've used this model in previous chapters, so by inspection you may recognize that we want\n",
|
||||
"\n",
|
||||
"$$\\mathbf F = \\left[\\begin{array}{cc|c} 1 & \\Delta t & 0\\\\\n",
|
||||
@ -497,7 +493,7 @@
|
||||
"\n",
|
||||
"Recall that $\\mathbf A$ is the *system dynamics matrix*. It describes a set of linear differential equations. From it we must compute the state transition matrix $\\mathbf F$. $\\mathbf F$ describes a discrete set of linear equations which compute $\\mathbf x$ for a discrete time step $\\Delta t$.\n",
|
||||
"\n",
|
||||
"and solve the following power series expansion of the matrix exponential to linearize the equations at $t$:\n",
|
||||
"A common way to compute $\\mathbf F$ is to use the power series expansion of the matrix exponential:\n",
|
||||
"\n",
|
||||
"$$\\mathbf F(\\Delta t) = e^{\\mathbf A\\Delta t} = \\mathbf{I} + \\mathbf A\\Delta t + \\frac{(\\mathbf A\\Delta t)^2}{2!} + \\frac{(\\mathbf A \\Delta t)^3}{3!} + ... $$\n",
|
||||
"\n",
|
||||
@ -508,7 +504,7 @@
|
||||
"\\begin{aligned}\n",
|
||||
"\\mathbf F(\\Delta t) &=\\mathbf{I} + \\mathbf At + \\mathbf{0} \\\\\n",
|
||||
"&= \\begin{bmatrix}1&0\\\\0&1\\end{bmatrix} + \\begin{bmatrix}0&1\\\\0&0\\end{bmatrix}\\Delta t\\\\\n",
|
||||
"&= \\begin{bmatrix}1&t\\\\0&1\\end{bmatrix}\n",
|
||||
"&= \\begin{bmatrix}1&\\Delta t\\\\0&1\\end{bmatrix}\n",
|
||||
"\\end{aligned}$$\n",
|
||||
"\n",
|
||||
"This give us\n",
|
||||
@ -518,25 +514,20 @@
|
||||
"\\mathbf{\\overline x} &=\\begin{bmatrix}1&\\Delta t\\\\0&1\\end{bmatrix}\\mathbf x\n",
|
||||
"\\end{aligned}$$\n",
|
||||
"\n",
|
||||
"This is the same result used by the kinematic equations! This exercise was unnecessary other than to illustrate linearizing differential equations. Subsequent examples will require you to use these techniques. "
|
||||
"This is the same result used by the kinematic equations! This exercise was unnecessary other than to illustrate finding the state transition matrix from linear differential equations. We will conclude the chapter with an example that will require the use of this technique."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Design the Measurement Model"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"The measurement function for our filter needs to take the filter state $\\mathbf x$ and turn it into a measurement, which is the slant range distance. We use the Pythagorean theorem to derive\n",
|
||||
"### Design the Measurement Model\n",
|
||||
"\n",
|
||||
"The measurement function takes the state estimate of the prior $\\overline{\\mathbf x}$ and turn it into a measurement of the slant range distance. For notational convenience I will use $\\mathbf x$, not $\\overline{\\mathbf x}$. We use the Pythagorean theorem to derive:\n",
|
||||
"\n",
|
||||
"$$h(\\mathbf x) = \\sqrt{x^2 + y^2}$$\n",
|
||||
"\n",
|
||||
"The relationship between the slant distance and the position on the ground is nonlinear due to the square root term. To use it in the EKF we must linearize it. As we discussed above, the best way to linearize an equation at a point is to find its slope, which we do by evaluatiing its partial derivative at a point:\n",
|
||||
"The relationship between the slant distance and the position on the ground is nonlinear due to the square root. We linearize it by evaluating its partial derivative at $\\mathbf x_t$:\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\mathbf H = \\frac{\\partial{h(\\mathbf x)}}{\\partial{\\mathbf x}}\\biggr|_{\\mathbf x_t}\n",
|
||||
@ -552,12 +543,10 @@
|
||||
"\\end{bmatrix}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"In other words, each element in the matrix is the partial derivative of the function $h$ with respect to the variables $x$. For our problem we have\n",
|
||||
"In other words, each element in the matrix is the partial derivative of the function $h$ with respect to the $x$ variables. For our problem we have\n",
|
||||
"\n",
|
||||
"$$\\mathbf H = \\begin{bmatrix}{\\partial h}/{\\partial x} & {\\partial h}/{\\partial \\dot{x}} & {\\partial h}/{\\partial y}\\end{bmatrix}$$\n",
|
||||
"\n",
|
||||
"where $h(x) = \\sqrt{x^2 + y^2}$.\n",
|
||||
"\n",
|
||||
"Solving each in turn:\n",
|
||||
"\n",
|
||||
"$$\\begin{aligned}\n",
|
||||
@ -590,9 +579,9 @@
|
||||
"\\frac{y}{\\sqrt{x^2 + y^2}}\n",
|
||||
"\\end{bmatrix}$$\n",
|
||||
"\n",
|
||||
"This may seem daunting, so step back and recognize that all of this math is doing something very simple. We have an equation for the slant range to the airplane which is nonlinear. The Kalman filter only works with linear equations, so we need to find a linear equation that approximates $\\mathbf H$. As we discussed above, finding the slope of a nonlinear equation at a given point is a good approximation. For the Kalman filter, the 'given point' is the state variable $\\mathbf x$ so we need to take the derivative of the slant range with respect to $\\mathbf x$. \n",
|
||||
"This may seem daunting, so step back and recognize that all of this math is doing something very simple. We have an equation for the slant range to the airplane which is nonlinear. The Kalman filter only works with linear equations, so we need to find a linear equation that approximates $\\mathbf H$. As we discussed above, finding the slope of a nonlinear equation at a given point is a good approximation. For the Kalman filter, the 'given point' is the state variable $\\mathbf x$ so we need to take the derivative of the slant range with respect to $\\mathbf x$. For the linear Kalman filter $\\mathbf H$ was a constant that we computed prior to running the filter. For the EKF $\\mathbf H$ is updated at each step as the evaluation point $\\overline{\\mathbf x}$ changes at each epoch.\n",
|
||||
"\n",
|
||||
"To make this more concrete, let's now write a Python function that computes the Jacobian of $\\mathbf H$ for this problem. The `ExtendedKalmanFilter` class will be using this to generate `ExtendedKalmanFilter.H` at each step of the process."
|
||||
"To make this more concrete, let's now write a Python function that computes the Jacobian of $h$ for this problem."
|
||||
]
|
||||
},
|
||||
{
|
||||
@ -688,36 +677,42 @@
|
||||
"source": [
|
||||
"### Design Process and Measurement Noise\n",
|
||||
"\n",
|
||||
"The radar returns the range distance. A good radar can achieve accuracy of $\\sigma_{range}= 5$ meters, so we will use that value. This gives us\n",
|
||||
"The radar measures the range to a target. We will use $\\sigma_{range}= 5$ meters for the noise. This gives us\n",
|
||||
"\n",
|
||||
"$$\\mathbf R = \\begin{bmatrix}\\sigma_{range}^2\\end{bmatrix} = \\begin{bmatrix}25\\end{bmatrix}$$\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"The design of $\\mathbf Q$ requires some discussion. The state $\\mathbf x= \\begin{bmatrix}x & \\dot x & y\\end{bmatrix}^\\mathtt{T}$. The first two elements are position (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 $\\mathbf Q$. The third element of $\\mathbf x$ is altitude, which we are assuming is independent of the down range distance. That leads us to a block design of $\\mathbf Q$ of:\n",
|
||||
"\n",
|
||||
"$$\\mathbf Q = \\begin{bmatrix}\\mathbf Q_\\mathtt{x} & 0 \\\\ 0 & \\mathbf Q_\\mathtt{y}\\end{bmatrix}$$\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"$$\\mathbf Q = \\begin{bmatrix}\\mathbf Q_\\mathtt{x} & 0 \\\\ 0 & \\mathbf Q_\\mathtt{y}\\end{bmatrix}$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Implementation\n",
|
||||
"\n",
|
||||
"The `FilterPy` library provides the class `ExtendedKalmanFilter`. It works very similar to the `KalmanFilter` class we have been using, except that it allows you to provide functions that compute the Jacobian of $\\mathbf H$ and the function $h(\\mathbf x)$. We have already written the code for these two functions, so let's get going.\n",
|
||||
"`FilterPy` provides the class `ExtendedKalmanFilter`. It works similarly to the `KalmanFilter` class we have been using, except that it allows you to provide a function that computes the Jacobian of $\\mathbf H$ and the function $h(\\mathbf x)$. \n",
|
||||
"\n",
|
||||
"We start by importing the filter and creating it. There are 3 variables in `x` and only 1 measurement. At the same time we will create our radar simulator.\n",
|
||||
"We start by importing the filter and creating it. The dimension of `x` is 3 and `z` has dimension 1.\n",
|
||||
"\n",
|
||||
"```python\n",
|
||||
"from filterpy.kalman import ExtendedKalmanFilter\n",
|
||||
"\n",
|
||||
"rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)\n",
|
||||
"```\n",
|
||||
"We create the radar simulator:\n",
|
||||
"```python\n",
|
||||
"radar = RadarSim(dt, pos=0., vel=100., alt=1000.)\n",
|
||||
"```\n",
|
||||
"\n",
|
||||
"We will initialize the filter near the airplane's actual position\n",
|
||||
"We will initialize the filter near the airplane's actual position:\n",
|
||||
"\n",
|
||||
"```python\n",
|
||||
"rk.x = array([radar.pos, radar.vel-10, radar.alt+100])\n",
|
||||
"```\n",
|
||||
"\n",
|
||||
"We assign the system matrix using the first term of the Taylor series expansion we computed above.\n",
|
||||
"We assign the system matrix using the first term of the Taylor series expansion we computed above:\n",
|
||||
"\n",
|
||||
"```python\n",
|
||||
"dt = 0.05\n",
|
||||
@ -726,7 +721,7 @@
|
||||
" [0, 0, 0]])*dt\n",
|
||||
"```\n",
|
||||
"\n",
|
||||
"After assigning reasonable values to $\\mathbf R$, $\\mathbf Q$, and $\\mathbf P$ we can run the filter with a simple loop\n",
|
||||
"After assigning reasonable values to $\\mathbf R$, $\\mathbf Q$, and $\\mathbf P$ we can run the filter with a simple loop. We pass the functions for computing the Jacobian of $\\mathbf H$ and $h(x)$ into the `update` method.\n",
|
||||
"\n",
|
||||
"```python\n",
|
||||
"for i in range(int(20/dt)):\n",
|
||||
@ -818,13 +813,8 @@
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Using SymPy to compute Jacobians"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## Using SymPy to compute Jacobians\n",
|
||||
"\n",
|
||||
"Depending on your experience with derivatives you may have found the computation of the Jacobian difficult. Even if you found it easy, a slightly more difficult problem easily leads to very difficult computations.\n",
|
||||
"\n",
|
||||
"As explained in Appendix A, we can use the SymPy package to compute the Jacobian for us."
|
||||
@ -882,23 +872,23 @@
|
||||
"source": [
|
||||
"## Robot Localization\n",
|
||||
"\n",
|
||||
"So, time to try a real problem. I warn you that this is far from a simple problem. However, most books choose simple, textbook problems with simple answers, and you are left wondering how to implement a real world solution. \n",
|
||||
"Time to try a real problem. I warn you that this section is difficult. However, most books choose simple, textbook problems with simple answers, and you are left wondering how to implement a solution for a real world problem. \n",
|
||||
"\n",
|
||||
"We will consider the problem of robot localization. We already implemented this in the **Unscented Kalman Filter** chapter, and I recommend you read that first. In this scenario we have a robot that is moving through a landscape with sensors that give range and bearings to various landmarks. This could be a self driving car using computer vision to identify trees, buildings, and other landmarks. It might be one of those small robots that vacuum your house, or a robot in a warehouse.\n",
|
||||
"We will consider the problem of robot localization. We already implemented this in the **Unscented Kalman Filter** chapter, and I recommend you read it now if you haven't already. We have a robot that is moving through a landscape, equipped with a sensor that give the range and bearing to landmarks. This could be a self driving car using computer vision to identify trees, buildings, and other landmarks. It might be one of those small robots that vacuum your house, or a robot in a warehouse.\n",
|
||||
"\n",
|
||||
"Our robot has 4 wheels configured the same as an automobile. It maneuvers by pivoting the front wheels. This causes the robot to pivot around the rear axle while moving forward. This is nonlinear behavior which we will have to model. \n",
|
||||
"Our robot has 4 wheels configured the same as an automobile's. It maneuvers by pivoting the front wheels. This causes the robot to pivot around the rear axle while moving forward. This is nonlinear behavior which we will have to model. \n",
|
||||
"\n",
|
||||
"The robot has a sensor that gives it approximate range and bearing to known targets in the landscape. This is nonlinear because computing a position from a range and bearing requires square roots and trigonometry. \n",
|
||||
"The robot has a sensor that measures the range and bearing to known targets in the landscape. This is nonlinear because computing a position from a range and bearing requires square roots and trigonometry. \n",
|
||||
"\n",
|
||||
"Both the process model and measurement models are nonlinear. The UKF accommodates both, so we provisionally conclude that the UKF is a viable choice for this problem.\n",
|
||||
"\n",
|
||||
"### Robot Motion Model"
|
||||
"Both the process model and measurement models are nonlinear. The EKF accommodates both, so we provisionally conclude that the EKF is a viable choice for this problem."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Robot Motion Model\n",
|
||||
"\n",
|
||||
"At a first approximation an automobile steers by pivoting the front tires while moving forward. The front of the car moves in the direction that the wheels are pointing while pivoting around the rear tires. This simple description is complicated by issues such as slippage due to friction, the differing behavior of the rubber tires at different speeds, and the need for the outside tire to travel a different radius than the inner tire. Accurately modeling steering requires a complicated set of differential equations. \n",
|
||||
"\n",
|
||||
"For Kalman filtering, especially for lower speed robotic applications a simpler *bicycle model* has been found to perform well. This is a depiction of the model:"
|
||||
@ -930,7 +920,7 @@
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"In the **Unscented Kalman Filter** chapter we derived these equations describing for this model:\n",
|
||||
"In the **Unscented Kalman Filter** chapter we derived the equations for this model:\n",
|
||||
"\n",
|
||||
"$$\\begin{aligned} x &= x - R\\sin(\\theta) + R\\sin(\\theta + \\beta) \\\\\n",
|
||||
"y &= y + R\\cos(\\theta) - R\\cos(\\theta + \\beta) \\\\\n",
|
||||
@ -949,7 +939,7 @@
|
||||
"source": [
|
||||
"### Design the State Variables\n",
|
||||
"\n",
|
||||
"For our robot we will maintain the position and orientation of the robot:\n",
|
||||
"For our filter we will maintain the position $x,y$ and orientation $\\theta$ of the robot:\n",
|
||||
"\n",
|
||||
"$$\\mathbf x = \\begin{bmatrix}x \\\\ y \\\\ \\theta\\end{bmatrix}$$\n",
|
||||
"\n",
|
||||
@ -964,7 +954,7 @@
|
||||
"source": [
|
||||
"### Design the System Model\n",
|
||||
"\n",
|
||||
"In general we model our system as a nonlinear motion model plus noise.\n",
|
||||
"We model our system as a nonlinear motion model plus noise.\n",
|
||||
"\n",
|
||||
"$$\\overline x = x + f(x, u) + \\mathcal{N}(0, Q)$$\n",
|
||||
"\n",
|
||||
@ -1016,7 +1006,8 @@
|
||||
"cell_type": "code",
|
||||
"execution_count": 10,
|
||||
"metadata": {
|
||||
"collapsed": false
|
||||
"collapsed": false,
|
||||
"scrolled": true
|
||||
},
|
||||
"outputs": [
|
||||
{
|
||||
@ -1199,7 +1190,7 @@
|
||||
"\\mathbf{\\overline P} &=\\mathbf{FPF}^{\\mathsf T} + \\mathbf{VMV}^{\\mathsf T}\n",
|
||||
"\\end{aligned}$$\n",
|
||||
"\n",
|
||||
"One final point. This form of linearization is not the only way to predict $\\mathbf x$. For example, we could use a numerical integration technique like *Runge Kutta* to compute the position of the robot in the future. In fact, if the time step is relatively large you will have to do that. As I am sure you are realizing, things are not as cut and dried with the EKF as it was for the KF. For a real problem you have to very carefully model your system with differential equations and then determine the most appropriate way to solve that system. The correct approach depends on the accuracy you require, how nonlinear the equations are, your processor budget, and numerical stability concerns. These are all topics beyond the scope of this book."
|
||||
"This form of linearization is not the only way to predict $\\mathbf x$. For example, we could use a numerical integration technique such as *Runge Kutta* to compute the position of the robot in the future. If the time step is relatively large this will be required. Things are not as cut and dried with the EKF as it was for the Kalman filter. For a real problem you have to carefully model your system with differential equations and then determine the most appropriate way to solve that system. The correct approach depends on the accuracy you require, how nonlinear the equations are, your processor budget, and numerical stability concerns."
|
||||
]
|
||||
},
|
||||
{
|
||||
@ -1208,11 +1199,11 @@
|
||||
"source": [
|
||||
"### Design the Measurement Model\n",
|
||||
"\n",
|
||||
"Now we need to design our measurement model. For this problem we are assuming that we have a sensor that receives a noisy bearing and range to multiple known locations in the landscape. The measurement model must convert the state $\\begin{bmatrix}x & y&\\theta\\end{bmatrix}^\\mathsf T$ into a range and bearing to the landmark. Using $p$ be the position of a landmark, the range $r$ is\n",
|
||||
"The robot's sensor provides a noisy bearing and range measurement to multiple known locations in the landscape. The measurement model must convert the state $\\begin{bmatrix}x & y&\\theta\\end{bmatrix}^\\mathsf T$ into a range and bearing to the landmark. Using $p$ be the position of a landmark, the range $r$ is\n",
|
||||
"\n",
|
||||
"$$r = \\sqrt{(p_x - x)^2 + (p_y - y)^2}$$\n",
|
||||
"\n",
|
||||
"We assume that the sensor provides bearing relative to the orientation of the robot, so we must subtract the robot's orientation from the bearing to get the sensor reading, like so:\n",
|
||||
"The sensor provides bearing relative to the orientation of the robot, so we must subtract the robot's orientation from the bearing to get the sensor reading, like so:\n",
|
||||
"\n",
|
||||
"$$\\phi = \\arctan(\\frac{p_y - y}{p_x - x}) - \\theta$$\n",
|
||||
"\n",
|
||||
@ -1333,18 +1324,23 @@
|
||||
" return Hx"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Design Measurement Noise\n",
|
||||
"\n",
|
||||
"It is reasonable to assume that the range and bearing measurement noise is independent, hence\n",
|
||||
"\n",
|
||||
"$$\\mathbf R=\\begin{bmatrix}\\sigma_{range}^2 & 0 \\\\ 0 & \\sigma_{bearing}^2\\end{bmatrix}$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {
|
||||
"collapsed": true
|
||||
},
|
||||
"source": [
|
||||
"### Design Measurement Noise\n",
|
||||
"\n",
|
||||
"This is quite straightforward as we need to specify measurement noise in measurement space, hence it is linear. It is reasonable to assume that the range and bearing measurement noise is independent, hence\n",
|
||||
"\n",
|
||||
"$$R=\\begin{bmatrix}\\sigma_{range}^2 & 0 \\\\ 0 & \\sigma_{bearing}^2\\end{bmatrix}$$\n",
|
||||
"\n",
|
||||
"### Implementation\n",
|
||||
"\n",
|
||||
"We will use `FilterPy`'s `ExtendedKalmanFilter` class to implement the filter. Its `predict()` method uses the standard linear equations. Our process model is nonlinear, so we will have to override `predict()` with our own version. I'll want to also use this class to simulate the robot, so I'll add a method `move()` that computes the position of the robot which both `predict()` and my simulation can call.\n",
|
||||
|
||||
Loading…
Reference in New Issue
Block a user