From d8ecbe5f6161edaf1c096754f8c167da449a6f50 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ariel=20Mart=C3=ADnez=20Silberstein?= Date: Wed, 7 Aug 2024 14:09:13 +0200 Subject: [PATCH 1/9] Fix Jacobian definitions --- 11-Extended-Kalman-Filters.ipynb | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/11-Extended-Kalman-Filters.ipynb b/11-Extended-Kalman-Filters.ipynb index ac78a5ac..a4493c71 100644 --- a/11-Extended-Kalman-Filters.ipynb +++ b/11-Extended-Kalman-Filters.ipynb @@ -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 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 \\mathbf 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", From 9d0c291070c003465c04c377345ffd8cc63921c4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ariel=20Mart=C3=ADnez=20Silberstein?= Date: Wed, 7 Aug 2024 14:15:28 +0200 Subject: [PATCH 2/9] Fix typo --- 11-Extended-Kalman-Filters.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/11-Extended-Kalman-Filters.ipynb b/11-Extended-Kalman-Filters.ipynb index a4493c71..5622daf5 100644 --- a/11-Extended-Kalman-Filters.ipynb +++ b/11-Extended-Kalman-Filters.ipynb @@ -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$. The [*Jacobian*](https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant) of a function a matrix containing all its first-order partial derivatives. 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", From 96eeaf535c3d782cc5989228d7dfc81a676f6fde Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ariel=20Mart=C3=ADnez=20Silberstein?= Date: Wed, 7 Aug 2024 14:27:25 +0200 Subject: [PATCH 3/9] Change H for h --- 11-Extended-Kalman-Filters.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/11-Extended-Kalman-Filters.ipynb b/11-Extended-Kalman-Filters.ipynb index 5622daf5..61c98567 100644 --- a/11-Extended-Kalman-Filters.ipynb +++ b/11-Extended-Kalman-Filters.ipynb @@ -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." ] From 022bd507e591e88fd9bdd85763939d2d6eaa6631 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ariel=20Mart=C3=ADnez=20Silberstein?= Date: Wed, 7 Aug 2024 14:28:00 +0200 Subject: [PATCH 4/9] Remove bold for function h --- 11-Extended-Kalman-Filters.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/11-Extended-Kalman-Filters.ipynb b/11-Extended-Kalman-Filters.ipynb index 61c98567..d84528e7 100644 --- a/11-Extended-Kalman-Filters.ipynb +++ b/11-Extended-Kalman-Filters.ipynb @@ -297,7 +297,7 @@ "\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", From ad8a78626e22bea56acce6d180ed52b224b3189b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ariel=20Mart=C3=ADnez=20Silberstein?= Date: Wed, 7 Aug 2024 14:32:25 +0200 Subject: [PATCH 5/9] Fix docstring of Jacobian --- 11-Extended-Kalman-Filters.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/11-Extended-Kalman-Filters.ipynb b/11-Extended-Kalman-Filters.ipynb index d84528e7..e6c06892 100644 --- a/11-Extended-Kalman-Filters.ipynb +++ b/11-Extended-Kalman-Filters.ipynb @@ -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", From 53286669d8929a3a673fc562535d235fbeb5116c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ariel=20Mart=C3=ADnez=20Silberstein?= Date: Wed, 7 Aug 2024 14:33:46 +0200 Subject: [PATCH 6/9] Fix unrelated typo --- 11-Extended-Kalman-Filters.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/11-Extended-Kalman-Filters.ipynb b/11-Extended-Kalman-Filters.ipynb index e6c06892..51432f61 100644 --- a/11-Extended-Kalman-Filters.ipynb +++ b/11-Extended-Kalman-Filters.ipynb @@ -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", From 8fbcc73e1be8eebb3d749dcb6341bfb3ebcb0465 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ariel=20Mart=C3=ADnez=20Silberstein?= Date: Wed, 7 Aug 2024 14:36:00 +0200 Subject: [PATCH 7/9] Remove 'of' word when refering to Jacobian H --- 11-Extended-Kalman-Filters.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/11-Extended-Kalman-Filters.ipynb b/11-Extended-Kalman-Filters.ipynb index 51432f61..be1a993f 100644 --- a/11-Extended-Kalman-Filters.ipynb +++ b/11-Extended-Kalman-Filters.ipynb @@ -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", From f6e10f02cf668784791f5504e23e405655d723f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ariel=20Mart=C3=ADnez=20Silberstein?= Date: Wed, 7 Aug 2024 14:38:34 +0200 Subject: [PATCH 8/9] Same as previous commit --- 11-Extended-Kalman-Filters.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/11-Extended-Kalman-Filters.ipynb b/11-Extended-Kalman-Filters.ipynb index be1a993f..7f5f10be 100644 --- a/11-Extended-Kalman-Filters.ipynb +++ b/11-Extended-Kalman-Filters.ipynb @@ -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", From 23fceb4987a69706c92bbb1dc33f5df84ef17f80 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ariel=20Mart=C3=ADnez=20Silberstein?= Date: Wed, 7 Aug 2024 15:19:13 +0200 Subject: [PATCH 9/9] Fix docstring of Jacobian --- 11-Extended-Kalman-Filters.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/11-Extended-Kalman-Filters.ipynb b/11-Extended-Kalman-Filters.ipynb index 7f5f10be..18b590ed 100644 --- a/11-Extended-Kalman-Filters.ipynb +++ b/11-Extended-Kalman-Filters.ipynb @@ -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",