Skip to content

Fix Jacobian definitions in EKF chapter #510

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 9 commits into
base: master
Choose a base branch
from
Open
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 9 additions & 9 deletions 11-Extended-Kalman-Filters.ipynb
Original file line number Diff line number Diff line change
@@ -141,7 +141,7 @@
"\n",
"$$\\begin{aligned}m &= f'(x=1.5) \\\\&= 2(1.5) - 2 \\\\&= 1\\end{aligned}$$ \n",
"\n",
"Linearizing systems of differential equations is similar. We linearize $f(\\mathbf x, \\mathbf u)$, and $h(\\mathbf x)$ by taking the partial derivatives of each to evaluate $\\mathbf F$ and $\\mathbf H$ at the point $\\mathbf x_t$ and $\\mathbf u_t$. We call the partial derivative of a matrix the [*Jacobian*](https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant). This gives us the the discrete state transition matrix and measurement model matrix:\n",
"Linearizing systems of differential equations is similar. We linearize $f(\\mathbf x, \\mathbf u)$, and $h(\\mathbf x)$ by taking the partial derivatives of each to evaluate $\\mathbf F$ and $\\mathbf H$ at the point $\\mathbf x_t$ and $\\mathbf u_t$. The [*Jacobian*](https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant) of a function is a matrix containing all its first-order partial derivatives. This gives us the the discrete state transition matrix and measurement model matrix:\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
@@ -295,9 +295,9 @@
"\\mathbf H = \\frac{\\partial{h(\\bar{\\mathbf x})}}{\\partial{\\bar{\\mathbf x}}}\\biggr|_{\\bar{\\mathbf x}_t}\n",
"$$\n",
"\n",
"The partial derivative of a matrix is called a Jacobian, and takes the form \n",
"The generalized partial derivative is a matrix called Jacobian, and takes the form \n",
"\n",
"$$\\frac{\\partial \\mathbf H}{\\partial \\bar{\\mathbf x}} = \n",
"$$\\frac{\\partial h}{\\partial \\bar{\\mathbf x}} = \n",
"\\begin{bmatrix}\n",
"\\frac{\\partial h_1}{\\partial x_1} & \\frac{\\partial h_1}{\\partial x_2} &\\dots \\\\\n",
"\\frac{\\partial h_2}{\\partial x_1} & \\frac{\\partial h_2}{\\partial x_2} &\\dots \\\\\n",
@@ -341,7 +341,7 @@
"\\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$. 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 $\\bar{\\mathbf x}$ changes at each epoch.\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 $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 $\\bar{\\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 $h$ for this problem."
]
@@ -354,7 +354,7 @@
"source": [
"from math import sqrt\n",
"def HJacobian_at(x):\n",
" \"\"\" compute Jacobian of H matrix at x \"\"\"\n",
" \"\"\" compute Jacobian matrix of h at x \"\"\"\n",
"\n",
" horiz_dist = x[0]\n",
" altitude = x[2]\n",
@@ -401,7 +401,7 @@
"\n",
"class RadarSim:\n",
" \"\"\" Simulates the radar signal returns from an object\n",
" flying at a constant altityude and velocity in 1D. \n",
" flying at a constant altitude and velocity in 1D. \n",
" \"\"\"\n",
" \n",
" def __init__(self, dt, pos, vel, alt):\n",
@@ -449,7 +449,7 @@
"source": [
"### Implementation\n",
"\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",
"`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 $\\mathbf H$ and the function $h(\\mathbf x)$. \n",
"\n",
"We start by importing the filter and creating it. The dimension of `x` is 3 and `z` has dimension 1.\n",
"\n",
@@ -477,7 +477,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. We pass the functions for computing the Jacobian of $\\mathbf H$ and $h(x)$ into the `update` method.\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 $\\mathbf H$ and $h(x)$ into the `update` method.\n",
"\n",
"```python\n",
"for i in range(int(20/dt)):\n",
@@ -1032,7 +1032,7 @@
"from math import sqrt\n",
"\n",
"def H_of(x, landmark_pos):\n",
" \"\"\" compute Jacobian of H matrix where h(x) computes \n",
" \"\"\" compute Jacobian matrix of h, where h(x) computes \n",
" the range and bearing to a landmark for state x \"\"\"\n",
"\n",
" px = landmark_pos[0]\n",