From b311ba176ec237bbd621f1d2d6d7cee4d5872ad7 Mon Sep 17 00:00:00 2001 From: nstathou Date: Fri, 10 Oct 2025 12:52:34 +0200 Subject: [PATCH 01/10] Fix missing $ for math --- notebooks/1_kalman_filters/4_ekf_slam.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/notebooks/1_kalman_filters/4_ekf_slam.ipynb b/notebooks/1_kalman_filters/4_ekf_slam.ipynb index 0dec390..dd4e60c 100644 --- a/notebooks/1_kalman_filters/4_ekf_slam.ipynb +++ b/notebooks/1_kalman_filters/4_ekf_slam.ipynb @@ -972,7 +972,7 @@ "id": "a82c24f8", "metadata": {}, "source": [ - "To keep the implementation modular, we define separate functions for computing $^\\text{low}H_t$ (the 2ร—5 observation Jacobian), the auxiliary matrix $F_{x,j}, and the full Jacobian $H_t$." + "To keep the implementation modular, we define separate functions for computing $^\\text{low}H_t$ (the 2ร—5 observation Jacobian), the auxiliary matrix $F_{x,j}$, and the full Jacobian $H_t$." ] }, { From b8cb6106cd2760f4dc6cd4b5df320143e8d6ff31 Mon Sep 17 00:00:00 2001 From: Nikos Stathoulopoulos <73775727+nstathou@users.noreply.github.com> Date: Sun, 12 Oct 2025 19:54:36 +0200 Subject: [PATCH 02/10] Fix typo --- notebooks/2_particle_filters/3_fast_slam.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/notebooks/2_particle_filters/3_fast_slam.ipynb b/notebooks/2_particle_filters/3_fast_slam.ipynb index 9cc45c9..af05614 100644 --- a/notebooks/2_particle_filters/3_fast_slam.ipynb +++ b/notebooks/2_particle_filters/3_fast_slam.ipynb @@ -89,7 +89,7 @@ "\n", "> ๐Ÿ“ **Note:** This was first introduced for SLAM by Murphy in 1999. \n", "\n", - "*Now, how do we **compute the map posterior** efficiently?*\n", + "*Now, how do we* ***compute the map posterior*** *efficiently?*\n", "\n", "Because we factor the problem into the robot path and the map, and compute the map given the poses, the **landmarks are conditionally independent given the poses**; hence, from Eq. (1) we can write:\n", "\n", From 254d7c83572e651ab1937658c6f724668dd9754d Mon Sep 17 00:00:00 2001 From: Nikos Stathoulopoulos <73775727+nstathou@users.noreply.github.com> Date: Sun, 12 Oct 2025 19:55:50 +0200 Subject: [PATCH 03/10] Fix typo --- notebooks/2_particle_filters/3_fast_slam.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/notebooks/2_particle_filters/3_fast_slam.ipynb b/notebooks/2_particle_filters/3_fast_slam.ipynb index af05614..54cf1bf 100644 --- a/notebooks/2_particle_filters/3_fast_slam.ipynb +++ b/notebooks/2_particle_filters/3_fast_slam.ipynb @@ -33,7 +33,7 @@ "\n", "*Can we exploit **dependencies between the different dimensions** of the state space?*\n", "\n", - "- *If we **know the poses** of the robot, **mapping is easy!***\n", + "- *If we* ***know the poses*** *of the robot,* ***mapping is easy!***\n", "\n", "**Key Idea:**\n", "\n", From ed8abb750849d4cc9d2dba59b4f84596faffa456 Mon Sep 17 00:00:00 2001 From: Nikos Stathoulopoulos <73775727+nstathou@users.noreply.github.com> Date: Sun, 12 Oct 2025 20:42:29 +0200 Subject: [PATCH 04/10] Remove 'ADD FIGURES HERE' placeholder text Removed placeholder text for figures in FastSLAM notebook. --- notebooks/2_particle_filters/3_fast_slam.ipynb | 2 -- 1 file changed, 2 deletions(-) diff --git a/notebooks/2_particle_filters/3_fast_slam.ipynb b/notebooks/2_particle_filters/3_fast_slam.ipynb index 54cf1bf..10d6dfd 100644 --- a/notebooks/2_particle_filters/3_fast_slam.ipynb +++ b/notebooks/2_particle_filters/3_fast_slam.ipynb @@ -150,8 +150,6 @@ " - **Action update:** sample a new pose using the proposal distribution (motion model).\n", " - **Sensor update:** EKF on observed landmarks.\n", "\n", - "ADD FIGURES HERE.\n", - "\n", "##### ๐Ÿ”‘ **Key Steps of FastSLAM 1.0** \n", "\n", "1. The first step is to **extend** the path posterior by **sampling** a new pose for each sample\n", From e35469267f4c450c280ce1cfeee2ddb048604a1b Mon Sep 17 00:00:00 2001 From: Nikos Stathoulopoulos <73775727+nstathou@users.noreply.github.com> Date: Sun, 12 Oct 2025 20:44:24 +0200 Subject: [PATCH 05/10] Fix typo --- notebooks/2_particle_filters/3_fast_slam.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/notebooks/2_particle_filters/3_fast_slam.ipynb b/notebooks/2_particle_filters/3_fast_slam.ipynb index 10d6dfd..cb65eb5 100644 --- a/notebooks/2_particle_filters/3_fast_slam.ipynb +++ b/notebooks/2_particle_filters/3_fast_slam.ipynb @@ -176,7 +176,7 @@ "source": [ "#### ๐Ÿ‘จ๐Ÿปโ€๐Ÿ’ป **FastSLAM 1.0 Algorithm**\n", "\n", - "The complete ****FastSLAM 1.0** algorithm can be summarized as follows:\n", + "The complete ***FastSLAM 1.0*** algorithm can be summarized as follows:\n", "\n", "> def **FastSLAM1.0($\\color{#ffa500}\\mathcal{X}_{t-1}, \\color{#ffa500}c_t, \\color{#ffa500}u_t, \\color{#ffa500}z_t$):** \n", ">>\n", From 4cfc3e28081e951fc86768ee8a0eb087ece1efab Mon Sep 17 00:00:00 2001 From: Nikos Stathoulopoulos <73775727+nstathou@users.noreply.github.com> Date: Sun, 12 Oct 2025 22:16:43 +0200 Subject: [PATCH 06/10] Fix typo --- notebooks/2_particle_filters/4_grid_based_slam.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/notebooks/2_particle_filters/4_grid_based_slam.ipynb b/notebooks/2_particle_filters/4_grid_based_slam.ipynb index 61616b5..1d22fdc 100644 --- a/notebooks/2_particle_filters/4_grid_based_slam.ipynb +++ b/notebooks/2_particle_filters/4_grid_based_slam.ipynb @@ -10,7 +10,7 @@ "***Questions:***\n", "\n", "- *Can we solve SLAM problem if **no pre-defined landmarks** are available?*\n", - "- ***Are the landmarks useful** to an autonomous robots other than for localization?*\n", + "- ***Are the landmarks useful** to autonomous robots other than for localization?*\n", "- *Can we use the ideas of FastSLAM to **build occupancy maps**?*\n", "\n", "**Reminder!**\n", From 639c89e2b9797ab2ea7279d6003b99333416eb0a Mon Sep 17 00:00:00 2001 From: Nikos Stathoulopoulos <73775727+nstathou@users.noreply.github.com> Date: Sun, 12 Oct 2025 23:22:32 +0200 Subject: [PATCH 07/10] Fix typo --- notebooks/2_particle_filters/4_grid_based_slam.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/notebooks/2_particle_filters/4_grid_based_slam.ipynb b/notebooks/2_particle_filters/4_grid_based_slam.ipynb index 1d22fdc..0ece105 100644 --- a/notebooks/2_particle_filters/4_grid_based_slam.ipynb +++ b/notebooks/2_particle_filters/4_grid_based_slam.ipynb @@ -889,7 +889,7 @@ "p(z_t \\mid x_{t-1}^{[i]}, m^{[i]}, u_t) = \\int{p(z_t \\mid x_t, m^{[i]}) \\cdot p(x_t \\mid x_{t-1}^{[i]}, u_t) \\cdot dx_t} \\quad\\quad (2)\n", "$$\n", "\n", - "Now we notice that the **inner part** of the integral is identical to the nominator in Eq. (1). We rewrite it simply as:\n", + "Now we notice that the **inner part** of the integral is identical to the numerator in Eq. (1). We rewrite it simply as:\n", "\n", "$$\n", "p(z_t \\mid x_t, m^{[i]}) \\cdot p(x_t \\mid x_{t-1}^{[i]}, u_t) = \\frac{\\tau(x)}{\\int{\\tau(x_t) \\cdot dx_t}}\n", From 0725da5e17d1dd0cd0729c873e0807c8817b1b76 Mon Sep 17 00:00:00 2001 From: Nikos Stathoulopoulos <73775727+nstathou@users.noreply.github.com> Date: Sun, 12 Oct 2025 23:28:16 +0200 Subject: [PATCH 08/10] Update mistake in equation in grid-based SLAM notebook --- notebooks/2_particle_filters/4_grid_based_slam.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/notebooks/2_particle_filters/4_grid_based_slam.ipynb b/notebooks/2_particle_filters/4_grid_based_slam.ipynb index 0ece105..5ee25ff 100644 --- a/notebooks/2_particle_filters/4_grid_based_slam.ipynb +++ b/notebooks/2_particle_filters/4_grid_based_slam.ipynb @@ -892,7 +892,7 @@ "Now we notice that the **inner part** of the integral is identical to the numerator in Eq. (1). We rewrite it simply as:\n", "\n", "$$\n", - "p(z_t \\mid x_t, m^{[i]}) \\cdot p(x_t \\mid x_{t-1}^{[i]}, u_t) = \\frac{\\tau(x)}{\\int{\\tau(x_t) \\cdot dx_t}}\n", + "p(x_t \\mid x_{t-1}^{[i]}, m^{[i]}, z_t, u_t) = \\frac{\\tau(x)}{\\int{\\tau(x_t) \\cdot dx_t}}\n", "$$\n", "\n", "Thus, we have **the same product** of the two terms where one dominates the other. \n", From 650738ddaaca7c819469808a24655552d190ec01 Mon Sep 17 00:00:00 2001 From: Nikos Stathoulopoulos <73775727+nstathou@users.noreply.github.com> Date: Mon, 13 Oct 2025 00:03:01 +0200 Subject: [PATCH 09/10] Fix HTML formatting in resampling explanation --- notebooks/2_particle_filters/4_grid_based_slam.ipynb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/notebooks/2_particle_filters/4_grid_based_slam.ipynb b/notebooks/2_particle_filters/4_grid_based_slam.ipynb index 5ee25ff..6130486 100644 --- a/notebooks/2_particle_filters/4_grid_based_slam.ipynb +++ b/notebooks/2_particle_filters/4_grid_based_slam.ipynb @@ -966,7 +966,7 @@ "source": [ "### ๐Ÿ” **Resampling** \n", "\n", - "Resampling at each step **limits the** ***\"memory\"*** of our filter. If we lose 25% of the particles at each time step, this may lead to **losing history of the poses** and therefore diversity in the particles.\n", + "Resampling at each step **limits the** ***\"memory\"*** of our filter. If we lose 25% of the particles at each time step, this may lead to **losing history of the poses** and therefore diversity in the particles.\n", "\n", "**Goal:**\n", "- Reduce the resampling actions.\n", From 8bdd6d63408883afb344de138f447270c1d8cd8e Mon Sep 17 00:00:00 2001 From: nikolaos stathoulopoulos Date: Tue, 14 Oct 2025 01:34:20 +0200 Subject: [PATCH 10/10] Least Squares --- notebooks/3_graph_based/1_least_squares.ipynb | 455 ++++++++++++++++++ 1 file changed, 455 insertions(+) create mode 100644 notebooks/3_graph_based/1_least_squares.ipynb diff --git a/notebooks/3_graph_based/1_least_squares.ipynb b/notebooks/3_graph_based/1_least_squares.ipynb new file mode 100644 index 0000000..5e09409 --- /dev/null +++ b/notebooks/3_graph_based/1_least_squares.ipynb @@ -0,0 +1,455 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "282d6c95", + "metadata": {}, + "source": [ + "## **Least Squares**\n", + "\n", + "The **Least Squares** method is one of the cornerstones of modern estimation and optimization in robotics. \n", + "It provides a simple yet powerful way to find the state that best fits a set of noisy measurements.\n", + "\n", + "Least Squares tries to **minimize the difference** between what we **measure** and what we **expect to measure**.\n", + "\n", + "- Originally used decades ago, but computationally too expensive for large systems. \n", + "\n", + "- With the rise of efficient solvers and sparse linear algebra in the 2010s, it made a strong comeback in SLAM and computer vision. \n", + "\n", + "- Today, it is the foundation of most **graph-based SLAM**, **bundle adjustment**, and **trajectory optimization** techniques.\n" + ] + }, + { + "cell_type": "markdown", + "id": "6396c252", + "metadata": {}, + "source": [ + "---" + ] + }, + { + "cell_type": "markdown", + "id": "3d552b2e", + "metadata": {}, + "source": [ + "\n", + "## ๐ŸŒ Least Squares in General \n", + "\n", + "The Least Squares is designed to compute a solution for an **overdetermined** system (*โ€œmore equations than unknownsโ€*). \n", + "\n", + "**Goal:**\n", + "\n", + "- Minimize the **sum of squared errors** in the equations. \n", + "\n", + "Standard approach for a large set of problems.\n", + "\n", + " **Example:** \n", + "\n", + "- In **regression models**, Least Squares is used to find the line or curve that best fits a set of observed data. \n", + "\n", + "### ๐Ÿงฉ **Problem Definition** \n", + "\n", + "Given a system described by a set of $n$ observation functions $\\{f_i(\\mathbf{x})\\}_{i=1:n}$\n", + "\n", + "**Let:**\n", + "\n", + "- $\\mathbf{x} \\;\\;$ be the **state vector**,\n", + "\n", + "- $\\mathbf{z}_i \\;\\;$ be a **measurement** of the state $\\mathbf{x}$,\n", + "\n", + "- $\\hat{\\mathbf{z}}_i = f_i(\\mathbf{x}) \\;\\;$ be a function which maps $\\mathbf{x}$ to a **predicted measurement** $\\hat{\\mathbf{z}}_i$.\n", + "\n", + "**Given:**\n", + "\n", + "- $n$ **noisy measurements** $\\mathbf{z}_{1:n}$ about the state $\\mathbf{x}$.\n", + "\n", + "**Goal:**\n", + "\n", + "- Estimate the state $\\mathbf{x}$ which **best explains the measurements** $\\mathbf{z}_{1:n}$." + ] + }, + { + "cell_type": "markdown", + "id": "b4acf6b9", + "metadata": {}, + "source": [ + "---" + ] + }, + { + "cell_type": "markdown", + "id": "3f61e2de", + "metadata": {}, + "source": [ + "### ๐Ÿงฉ **Error Function** \n", + "\n", + "The **error** $\\mathbf{e}_i$ is typically the **difference** between the **predicted** and **actual** measurement:\n", + "\n", + "$$\n", + "\\mathbf{e}_i(\\mathbf{x}) = \\mathbf{z}_i - f_i(\\mathbf{x})\n", + "$$\n", + "\n", + "- We assume the error has **zero mean** and is **normally distributed**.\n", + "\n", + "- Gaussian error with **information matrix** $\\mathbf{\\Omega}_i$.\n", + "\n", + "- The **squared error** of a measurement depends only on the state and is a scalar:\n", + "\n", + "$$\n", + "e_i(\\mathbf{x}) = \\mathbf{e}_i(\\mathbf{x})^{T}\\,\\mathbf{\\Omega}_i\\,\\mathbf{e}_i(\\mathbf{x})\n", + "$$" + ] + }, + { + "cell_type": "markdown", + "id": "2e20f64a", + "metadata": {}, + "source": [ + "\n", + "### ๐Ÿ“‰ **Find the Minimum**\n", + "\n", + "Find the state $\\mathbf{x}^\\star$ which **minimizes the error** given all measurements:\n", + "\n", + "$$\n", + "\\begin{aligned}\n", + "\\mathbf{x}^* &= \\arg\\min_\\mathbf{x} F(\\mathbf{x}) \\\\\n", + " &= \\arg\\min_\\mathbf{x} \\sum_i e_i(\\mathbf{x}) \\\\\n", + " &= \\arg\\min_\\mathbf{x} \\sum_i \\mathbf{e}_i(\\mathbf{x})^T \\, \\mathbf{\\Omega}_i \\, \\mathbf{e}_i(\\mathbf{x}).\n", + "\\end{aligned}\n", + "$$\n", + "\n", + "where:\n", + "\n", + "- $\\mathbf{\\Omega}_i$ represents our **uncertainty** in the measurements.\n", + "\n", + "- A general solution is to **derive the global error function** and find its nulls.\n", + "\n", + "- In general this is complex with **no closed-form** solution.\n", + "\n", + " **โ†ณ** Use **Numerical Approaches**.\n", + "\n", + "> ๐Ÿ“ **Note**: In a Gaussian density, $\\:\\: p(\\mathbf{x}) \\propto \\exp\\!\\left(-\\tfrac{1}{2}(\\mathbf{x}-\\boldsymbol{\\mu})^{T}\\,\\mathbf{\\Sigma}^{-1}(\\mathbf{x}-\\boldsymbol{\\mu})\\right)$, the exponent contains the same **quadratic form** as in least squares: $\\:\\:\\mathbf{e}^{T}\\mathbf{\\Omega}\\mathbf{e}\\:$ with $\\:\\:\\mathbf{\\Omega} \\rightarrow \\mathbf{\\Sigma}^{-1}$. Therefore, **minimizing the quadratic error** is similar to **maximizing the Gaussian likelihood**. \n", + "\n", + "#### ๐Ÿค” Assumptions\n", + "\n", + "- A **good initial guess** is available. \n", + "\n", + "- The error functions are ***โ€œsmoothโ€*** in the neighborhood of the *(hopefully)* global minima.\n", + "\n", + " **โ†ณ** Then we can solve by **iterative local linearization**." + ] + }, + { + "cell_type": "markdown", + "id": "747f4feb", + "metadata": {}, + "source": [ + "\n", + "#### Solve via Iterative Local Linearizations\n", + "\n", + "1. **Linearize** the error terms around the current solution (**initial guess**).\n", + "\n", + "2. Compute the **first derivative** of the squared error function.\n", + "\n", + "3. Set it to **zero** and solve a **linear system**.\n", + "\n", + "4. Obtain the **new state** (hopefully closer to the minimum).\n", + "\n", + "5. **Iterate.**\n", + "\n", + "#### Linearize the Error Function\n", + "\n", + "Approximate the error functions **around an initial guess** $\\mathbf{x}$ via a Taylor expansion:\n", + "\n", + "$$\n", + "\\mathbf{e}_i(\\mathbf{x} + \\Delta \\mathbf{x}) \\approx \\mathbf{e}_i + \\mathbf{J}_i(\\mathbf{x})\\,\\Delta \\mathbf{x},\n", + "$$\n", + "\n", + "where:\n", + "\n", + "- $\\mathbf{J}_i$ is the Jacobian of $\\mathbf{e}_i$ w.r.t. $\\mathbf{x}$.\n", + "\n", + "#### Squared Error\n", + "\n", + "- With the previous linearization, we fix $\\mathbf{x}$ and carry out the **minimization in the increments** $\\Delta \\mathbf{x}$. \n", + "\n", + "- We **replace** the Taylor expansion in the squared error terms as follows:\n", + "\n", + "Let:\n", + "\n", + "$$\n", + "\\mathbf{e}_i(\\mathbf{x}) = \\mathbf{z}_i - f_i(\\mathbf{x}),\\qquad\n", + "e_i(\\mathbf{x}) = \\mathbf{e}_i(\\mathbf{x})^{T}\\,\\mathbf{\\Omega}_i\\, \\mathbf{e}_i(\\mathbf{x}),\\qquad\n", + "F(\\mathbf{x})=\\sum_i e_i(\\mathbf{x}).\n", + "$$\n", + "\n", + "Linearize:\n", + "$$\n", + "\\mathbf{e}_i(\\mathbf{x}+\\Delta \\mathbf{x}) \\approx \\mathbf{e}_i(\\mathbf{x}) + \\mathbf{J}_i\\,\\Delta \\mathbf{x}.\n", + "$$\n", + "\n", + "Substitute into $e_i$:\n", + "\n", + "$$\n", + "\\begin{aligned}\n", + "e_i(x) &\\approx \\big(\\mathbf{e}_i + \\mathbf{J}_i\\,\\Delta \\mathbf{x}\\big)^{T}\\,\\mathbf{\\Omega}_i\\,\\big(\\mathbf{e}_i + \\mathbf{J}_i\\,\\Delta \\mathbf{x}\\big) \\\\\n", + "&= \\mathbf{e}_i^T\\;\\mathbf{\\Omega}\\;\\mathbf{e}_i + \\Delta \\mathbf{x}\\;\\mathbf{J}_i\\;\\mathbf{\\Omega}_i\\;\\mathbf{e}_i + \\mathbf{e}_i^T\\;\\mathbf{\\Omega}_i\\;\\mathbf{J}_i\\;\\Delta\\mathbf{x} + \\Delta\\mathbf{x}^T \\;\\mathbf{J}_i\\;\\mathbf{\\Omega}_i\\;\\mathbf{J}_i\\;\\Delta\\mathbf{x}\\\\\n", + "&= \\underbrace{\\mathbf{e}_i^T\\;\\mathbf{\\Omega}\\;\\mathbf{e}_i}_{={c}_i} + \\Delta\\mathbf{x}^T \\underbrace{\\;\\mathbf{J}_i\\;\\mathbf{\\Omega}_i\\;\\mathbf{J}_i}_{=\\mathbf{H}_i}\\;\\Delta\\mathbf{x} + 2\\underbrace{\\;\\mathbf{e}_i^T\\;\\mathbf{\\Omega}_i\\;\\mathbf{J}_i}_{=\\mathbf{b}_i^T}\\;\\Delta\\mathbf{x}\n", + "\\end{aligned}\n", + "$$\n", + "\n", + "Hence\n", + "\n", + "$$\n", + "e_i(x) \\approx c_i + 2\\,\\mathbf{b}_i^T\\;\\Delta\\mathbf{x} + \\Delta\\mathbf{x}^T \\;\\mathbf{H}_i\\;\\Delta\\mathbf{x}\n", + "$$" + ] + }, + { + "cell_type": "markdown", + "id": "f3ea3c3e", + "metadata": {}, + "source": [ + "\n", + "#### Global Error\n", + "\n", + "- The global error is the **sum of the squared error** terms corresponding to the individual measurements.\n", + "\n", + "- For a new expression which approximates the global error in the **neighborhoud of the current solution** $\\mathbf{x}$:\n", + "\n", + "From $\\:\\: F(\\mathbf{x})=\\sum_i e_i(\\mathbf{x})$, $\\;$ we get:\n", + "\n", + "$$\n", + "\\begin{aligned}\n", + "F(\\mathbf{x} + \\Delta\\mathbf{x}) &\\approx \\sum_i \\Big( c_i \\;+\\; 2\\,\\mathbf{b}_i^T\\;\\Delta\\mathbf{x} \\;+\\; \\Delta\\mathbf{x}^T \\;\\mathbf{H}_i\\;\\Delta\\mathbf{x}\\Big) \\\\\n", + "& = \\underbrace{\\sum_i c_i}_{c} + 2\\;(\\underbrace{\\sum_i \\mathbf{b}_i}_{\\mathbf{b}})^T\\Delta\\mathbf{x} + \\Delta \\mathbf{x}^T\\Big(\\underbrace{\\sum_i \\mathbf{H}_i}_{\\mathbf{H}}\\Big)\\Delta\\mathbf{x} \\\\\n", + "& = c + 2\\mathbf{b}^T\\Delta\\mathbf{x} + \\Delta\\mathbf{x}^T\\mathbf{H}\\Delta\\mathbf{x}\n", + "\\end{aligned}\n", + "$$\n", + "\n", + "with:\n", + "\n", + "$$\n", + "\\begin{aligned}\n", + "\\mathbf{b}^T &= \\sum_i \\mathbf{e}_i^T\\mathbf{\\Omega}_i\\mathbf{J}_i \\\\\n", + "\\mathbf{H} &= \\sum_i \\mathbf{J}_i^T\\mathbf{\\Omega}\\mathbf{J}_i\n", + "\\end{aligned}\n", + "$$" + ] + }, + { + "cell_type": "markdown", + "id": "507396b8", + "metadata": {}, + "source": [ + "---" + ] + }, + { + "cell_type": "markdown", + "id": "d62f9f47", + "metadata": {}, + "source": [ + "\n", + "### โœ๏ธ **Quadratic Form Minimization**\n", + "\n", + "Based on the above, we can write the global error term as a **quadratic form** in $\\Delta\\mathbf{x}$:\n", + "\n", + "$$\n", + "F(\\mathbf{x} + \\Delta\\mathbf{x}) \\approx c + 2\\mathbf{b}^T\\Delta\\mathbf{x} + \\Delta\\mathbf{x}^T\\mathbf{H}\\Delta\\mathbf{x}\n", + "$$\n", + "\n", + "The **approximated dervation** of $F(\\mathbf{x} + \\Delta\\mathbf{x})$ w.r.t. $\\Delta\\mathbf{x}$ is then:\n", + "\n", + "$$\n", + "\\frac{\\partial F(\\mathbf{x} + \\Delta\\mathbf{x})}{\\partial \\Delta\\mathbf{x}} \\approx 2\\mathbf{b} + 2\\mathbf{H}\\,\\Delta\\mathbf{x}.\n", + "$$\n", + "\n", + "Setting it **to zero** (minimum condition):\n", + "\n", + "$$\n", + "0 = 2\\mathbf{b} + 2\\mathbf{H}\\Delta\\mathbf{x}\n", + "$$\n", + "\n", + "Which leads to the **linear system**:\n", + "\n", + "$$\n", + "\\mathbf{H} \\Delta\\mathbf{x} = -\\mathbf{b} \n", + "$$\n", + "\n", + "The solution for the increment $$\\Delta\\mathbf{x}^*$ is:\n", + "\n", + "$$\n", + "\\Delta\\mathbf{x}^* = -\\mathbf{H}^{-1}\\mathbf{b}\n", + "$$\n", + "\n", + "---\n" + ] + }, + { + "cell_type": "markdown", + "id": "745a3cd2", + "metadata": {}, + "source": [ + "\n", + "### ๐ŸŽ“ Gauss-Newton Solution\n", + "\n", + "**Iterate the following steps:**\n", + "\n", + "1. **Linearize** around $\\mathbf{x}$ and compute for each measurement:\n", + " $$\n", + " \\mathbf{e}_i(\\mathbf{x} + \\Delta\\mathbf{x}) \\approx \\mathbf{e}_i(\\mathbf{x}) + \\mathbf{J}_i \\Delta\\mathbf{x}.\n", + " $$\n", + "\n", + "2. **Compute the terms** for the linear system:\n", + " $$\n", + " \\mathbf{b} = \\sum_i \\mathbf{e}_i^T \\mathbf{\\Omega}_i\\, \\mathbf{J}_i, \n", + " \\qquad\n", + " \\mathbf{H} = \\sum_i \\mathbf{J}_i^T \\mathbf{\\Omega}_i\\,\\mathbf{J}_i.\n", + " $$\n", + "\n", + "3. **Solve** the linear system:\n", + " $$\n", + " \\mathbf{H} \\Delta \\mathbf{x}^* = -\\mathbf{b} \\quad\\Rightarrow\\quad \\Delta \\mathbf{x}^* = -\\mathbf{H}^{-1}\\mathbf{b}.\n", + " $$\n", + "\n", + "4. **Update state:** \n", + "\n", + "$$\n", + "\\mathbf{x} \\leftarrow \\mathbf{x} + \\Delta \\mathbf{x}^*\n", + "$$\n" + ] + }, + { + "cell_type": "markdown", + "id": "9f5b6c8a", + "metadata": {}, + "source": [ + "---" + ] + }, + { + "cell_type": "markdown", + "id": "3d6e7f78", + "metadata": {}, + "source": [ + "\n", + "#### โœ”๏ธ Gauss-Newton Summary\n", + "\n", + "Method to **minimize a sum of squared errors**.\n", + "\n", + "- Start with an **initial guess**. \n", + "\n", + "- **Linearize** the individual error functions. \n", + "\n", + "- This leads to a **quadratic form**. \n", + "\n", + "- Obtain a **linear system** by setting its derivative to **zero**. \n", + "\n", + "- Solving the linear system leads to a **state update**. \n", + "\n", + "- **Iterate.**" + ] + }, + { + "cell_type": "markdown", + "id": "307d4a78", + "metadata": {}, + "source": [ + "---" + ] + }, + { + "cell_type": "markdown", + "id": "3d63deae", + "metadata": {}, + "source": [ + "\n", + "## ๐Ÿ–‡๏ธ **Relation to Probabilistic State Estimation**\n", + "\n", + "So far, we minimized an error function. \n", + "\n", + "*How does this relate to state estimation in the **probabilistic** sense?*\n", + "\n", + "### ๐Ÿงฎ **General State Estimation**\n", + "\n", + "Using Bayesโ€™ rule, independence, and the Markov assumption, we can write:\n", + "\n", + "$$\n", + "p(x_{0:t}\\mid z_{1:t}, u_{1:t}) \\propto p(x_0)\\,\\prod_{t}\\,p(x_t\\mid x_{t-1},u_t)\\,p(z_t\\mid x_t).\n", + "$$\n", + "\n", + "Written as the **Log-Likelihood**:\n", + "\n", + "$$\n", + "\\log p(x_{0:t}\\mid z_{1:t}, u_{1:t}) = \\text{const.} + \\log p(x_0) + \\sum_t \\big[\\log p(x_t\\mid x_{t-1},u_t) + \\log p(z_t\\mid x_t)\\big].\n", + "$$\n", + "\n", + "Assuming **Gaussian distributions**, for a Gaussian $\\mathcal N(x;\\mu,\\Sigma)$:\n", + "\n", + "$$\n", + "\\begin{aligned}\n", + "\\log \\mathcal N(x;\\mu,\\Sigma) &= \\text{const.} - \\tfrac{1}{2}\\,\\underbrace{(x-\\mu)^T}_{\\mathbf{e}^T(x)}\\,\\underbrace{\\Sigma^{-1}}_{\\mathbf{\\Omega}}\\,\\underbrace{(x-\\mu)}_{\\mathbf{e}(x)}\\\\\n", + "&= \\text{const.} - \\tfrac{1}{2}\\,\\underbrace{\\mathbf{e}(x)^T\\,\\mathbf{\\Omega}\\,\\mathbf{e}(x)}_{\\text{quadratic error } e(x)},\n", + "\\end{aligned}\n", + "$$\n", + "\n", + "Thus, up to a constant, the log-likelihood is equivalent to the **error functions** used before.\n", + "\n", + "Therefore,\n", + "\n", + "$$\n", + "\\log p(x_{0:t}\\mid z_{1:t}, u_{1:t}) =\\text{const.} - \\tfrac{1}{2}\\,e_p(x) - \\tfrac{1}{2}\\sum_t \\big(e_{u_t}(x) + e_{z_t}(x)\\big),\n", + "$$\n", + "\n", + "where $e_p$ is the **prior term**, $e_{u_t}$ the **motion** (odometry) **error**, and $e_{z_t}$ the **measurement error**.\n", + "\n", + "**Maximizing** the log-likelihood leads to:\n", + "\n", + "$$\n", + "\\arg\\max_x \\log p(x_{0:t}\\mid z_{1:t}, u_{1:t}) \\equiv \\arg\\min_x \\Big( e_p(x) + \\sum_t \\big[ e_{u_t}(x) + e_{z_t}(x) \\big] \\Big).\n", + "$$\n", + "\n", + "\n", + "**Takeaway:**\n", + "\n", + "- **Least squares** (with Gaussian assumptions) is **equivalent to Maximum A Posteriori** (MAP) estimation. \n", + "\n", + "- **Minimizing the sum of weighted squared residuals** corresponds to **maximizing the posterior probability**." + ] + }, + { + "cell_type": "markdown", + "id": "0761050c", + "metadata": {}, + "source": [ + "---" + ] + }, + { + "cell_type": "markdown", + "id": "87d5037c", + "metadata": {}, + "source": [ + "#### โœ”๏ธ Summary\n", + "\n", + "- Technique to **minimize squared error** functions.\n", + "\n", + "- Gauss-Newton is an **iterative approach** for non-linear problems.\n", + "\n", + "- Uses linearization (approximation).\n", + "\n", + "- Equivalent to **maximizing the log likelihood** of independent Gaussians.\n", + "\n", + "- Popular method in a lot of disciplines." + ] + } + ], + "metadata": { + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +}