In common engineering problems, the optimization problem is to minimize the residual of a parametric model with respect to some observed data points.

Preliminaries

Given $N$ measurements $\left\{\left(\mathbf{x}_i, \mathbf{y}_i\right)\right\}_{i=1}^N$ where $\mathbf{x}_i\in\mathbb{R}^p$ and $\mathbf{y}_i\in\mathbb{R}^{m_i}$. We define a prediction model parametrized by $\boldsymbol{\theta}\in\mathbb{R}^n$ as $\mathbf{h}\left(\cdot;\boldsymbol{\theta}\right):\mathbb{R}^p\to\mathbb{R}^{m_i}$ and the residual of a single datapoint as $\mathbf{r}_i:\mathbb{R}^n\to\mathbb{R}^{m_i}$, where $\mathbf{r}_i\left(\boldsymbol{\theta}\right)\coloneqq \mathbf{h}\left(\mathbf{x}_i;\boldsymbol{\theta}\right)-\mathbf{y}_i$. The full residual $\mathbf{r}:\mathbb{R}^{n}\to\mathbb{R}^m$ is simply a stack of every single residuals, i.e.,

Note that we require 𝑚≥𝑛 because otherwise the prediction model cannot be fully defined, i.e., there are infinitely many 𝜃 that perfectly fit the data.

The goal of an optimization problem is to find a model that best fits the data. To formalize this, we need to define some function $f:\mathbb{R}^n\to\mathbb{R}_{\geq 0}$ that properly maps the residual vector to a minimizable scalar, which is known as the objective function. In general, the function $f$ is assumed to be continuously differentiable. Some of the common objective functions are

• Least Squares

• Least Squares with $\ell_1$ regularization

• Least Squares with $\ell_2$ regularization

• Weighted Least Squares

• Cauchy Loss

The following is a list of some of the theorems related to optimality conditions. Suppose $f$ is differentiable, and define the gradient (first-order derivative) and Hessian (second-order derivative) as

1. Descent direction: If there is a vector $\mathbf{d}\in\mathbb{R}^n$ such that $\nabla f(\boldsymbol{\theta}_0)^\top\mathbf{d}<0$, then for all sufficiently small $\lambda>0$, $f(\boldsymbol{\theta}_0+\lambda\mathbf{d})<f(\boldsymbol{\theta}_0)$. Here, $\mathbf{d}$ is a descent direction of $f$ at $\boldsymbol{\theta}_0$.

   Intuitively, the dot product less than 0 means that vector $\mathbf{d}$ is in the opposite direction as the gradient.

2. First-order necessary condition: If $\boldsymbol{\theta}^*$ is a local minimum, then $\nabla f(\boldsymbol{\theta}^*)=0$.

3. Second-order necessary condition: If $\boldsymbol{\theta}^*$ is a local minimum, then $\nabla f(\boldsymbol{\theta}^*)=0$ and $\mathbf{H}(\boldsymbol{\theta}^*)\succeq 0$ (positive semi-definite).

4. Second-order sufficient condition: If $\nabla f(\boldsymbol{\theta}^*)=0$ and $\mathbf{H}(\boldsymbol{\theta}^*)\succ 0$ (positive definite), then $\boldsymbol{\theta}^*$ is a local minimum.

5. Convex function: $f$ is convex if and only if $\mathbf{H}(\boldsymbol{\theta})\succeq 0$ for all $\boldsymbol{\theta}$.

6. Global minimum: Suppose $f$ is convex, then $\boldsymbol{\theta}^\star$ is a global minimum if an only if $\nabla f(\boldsymbol{\theta}^\star)=0$.

Linear Least Squares

Let $\mathbf{A}\in\mathbb{R}^{m\times n}$ be a matrix and $\mathbf{b}\in\mathbb{R}^m$ be a vector, where $m>n$ and $\mathrm{rank}(\mathbf{A})=n$. The Linear Least Squares (LLS) optimization problem is to find the optimal solution $\mathbf{x}\in\mathbb{R}^n$ to the linear system of equations $\mathbf{A}\mathbf{x}=\mathbf{b}$. Define the objective function as

and the goal is to find $\mathbf{x}^{\star}=\argmin_{\mathbf{x}\in\mathbb{R}^n}f(\mathbf{x})$.

First, we can write out the gradient and Hessian matrix of $f$, which are

• Derivation

  Simply expand the objective function,

  $$\begin{align*} f(\mathbf{x})&=\frac{1}{2}\left(\mathbf{Ax-b}\right)^\top\left(\mathbf{Ax-b}\right)\\ &=\frac{1}{2}\left(\mathbf{x}^\top\mathbf{A}^\top-\mathbf{b}^\top\right)\left(\mathbf{Ax-b}\right)\\ &=\frac{1}{2}\left(\mathbf{x^\top A^\top Ax}-\mathbf{x^\top A^\top b}-\mathbf{b^\top Ax}+\mathbf{b^\top b}\right)\\ &=\frac{1}{2}\mathbf{x^\top A^\top Ax}-\mathbf{x^\top A^\top b}+\frac{1}{2}\mathbf{b^\top b} \end{align*}$$

  Note that the last step is true because $b^\top Ax\in\mathbb{R}$ and $\mathbf{b^\top Ax=(b^\top Ax)^\top=x^\top A^\top b}$.

Further notice that because of $\mathrm{rank}(\mathbf{A})=n$, the Hessian matrix is positive definite (and thus invertible), i.e., $\mathbf{A^\top A}\succ 0$. Then $\mathbf{x}^\star$ is the global minimum if and only if $\nabla f(\mathbf{x}^\star)=0$,

Although the above equation gives a closed-form formula for finding the global minimum $\mathbf{x}^\star$, computing the inverse of $\mathbf{A^\top A}$ is not practical. Instead, we solve a linear system of equations

which is known as the normal equations.

Therefore, solving an LLS problem is basically solving a linear system of equations. Typical methods include Cholesky decomposition and QR factorization, rather than directly inverting $\mathbf{A^\top A}$.

Cholesky Solver

The Cholesky decomposition of a positive-definite matrix $\mathbf{A}\in\mathbb{R}^{n\times n}$ is a decomposition into the product of an upper triangular matrix and a lower triangular matrix, i.e., $\mathbf{A}=\mathbf{LL^\top}$, where

The elements of $\mathbf{L}$ can be obtained as follows

For a linear system of equations, $\mathbf{Ax}=\mathbf{b}$, apply Cholesky decomposition and get $\mathbf{LL^\top x=b}$. Then, we first let $\mathbf{y}=\mathbf{L^\top x}$, and solve $\mathbf{Ly=b}$ for $\mathbf{y}$ using forward substitution ($y_1\to y_2\to\cdots \to y_n$). Specifically,

Note that $y_1$ can be easily found as $y_1 = b_1/l_{11}$, and substituting $y_1$ into the second equation solves $y_2$. In general,

Finally, solve $\mathbf{L^\top x}=\mathbf{y}$ for $\mathbf{x}$ using backward substitution ($x_n\to x_{n-1}\to\cdots\to x_1$). Specifically,

Now, $x_n$ can be easily found as $x_n=y_n/l_{nn}$, and substituting $x_n$ into the second to last equation solves $x_{n-1}$. In general,

In practice, we can use numpy.linalg.cholesky or similar packages to perform Cholesky decomposition. The time complexity is $O(n^3)$.

QR Solver

The QR factorization of a matrix $\mathbf{A}\in\mathbb{R}^{n\times n}$ is a decomposition into an orthogonal matrix and an upper triangular matrix, i.e., $\mathbf{A}=\mathbf{QR}$, where

where the element of $\mathbf{Q}$ can be found via Gram-Schmidt diagonalization.

For a linear system of equations, $\mathbf{Ax}=\mathbf{b}$, apply QR factorization and get $\mathbf{QRx=b}$. Multiply both sides by $\mathbf{Q}^\top$and get $\mathbf{Rx}=\mathbf{Q^\top b}$. Let $\mathbf{y}=\mathbf{Q^\top b}$, we can solve $\mathbf{Rx=y}$ for $\mathbf{x}$ by backward substitution, i.e.,

Non-linear Least Squares

In general, let $\mathbf{r}:\mathbb{R}^n\to \mathbb{R}^m,m\geq n$ be a smooth residual function. We can define an objective function $f:\mathbb{R}^n\to\mathbb{R}$ as

The goal is still to find the minimizer of the function $f$, i.e., $\mathbf{x}^{\star}=\argmin_{\mathbf{x}\in\mathbb{R}^n}f(\mathbf{x})$. Unfortunately, there’s no closed-form formula for finding $\mathbf{x}^\star$ as we did for the LLS problem. Common methods usually start from an initial guess, and iteratively approximate the optimum based on “descent direction” mentioned before.

Specifically, if we start with an initial guess $\mathbf{x}^0$, linearize around $\mathbf{x}^0$, and find a descending direction $\mathbf{d}$. Then, we update the guess by moving along direction $\mathbf{d}$ , and we can expect the objective will become smaller, i.e., $\mathbf{x}^{k+1 }=\mathbf{x}^k+\mathbf{d}$ and $f(\mathbf{x}^{k+1})<f(\mathbf{x}^k)$. Hopefully, this process will converge to a local minimum. Common methods include Gauss-Newton, Levenberg-Marquardt, or gradient descent, distinguished by how they compute $\mathbf{d}$.

Gauss-Newton Method

The steps of the Gauss-Newton method are as follows:

1. Start from an initial guess $\mathbf{x}^0$, iterate until convergence $k=0, 1, 2, \dots$

2. Linearize the residual at the current guess $\mathbf{x}^k$ using first-order Taylor expansion,

   $$\mathbf{r}(\mathbf{x}^k+\mathbf{d})=\mathbf{r}(\mathbf{x}^k)+\mathbf{J}(\mathbf{x}^k)\mathbf{d}$$

   where

   $$\begin{equation*} \mathbf{J}(\mathbf{x}) := \frac{\partial \mathbf{r(x)}}{\partial \mathbf{x}} = \begin{bmatrix} \dfrac{\partial r_1}{\partial x_1} & \dfrac{\partial r_1}{\partial x_2} & \cdots & \dfrac{\partial r_1}{\partial x_n}\\[12pt] \dfrac{\partial r_2}{\partial x_1} & \dfrac{\partial r_2}{\partial x_2} & \cdots & \dfrac{\partial r_2}{\partial x_n} \\[6pt] \vdots & \vdots & \ddots & \vdots \\[6pt] \dfrac{\partial r_m}{\partial x_1} & \dfrac{\partial r_m}{\partial x_2} & \cdots & \dfrac{\partial r_m}{\partial x_n} \end{bmatrix} \in \mathbb{R}^{m \times n} \end{equation*}$$

   is the Jacobian matrix. Let $\mathbf{J}_k\coloneqq \mathbf{J}(\mathbf{x}^k)$.

3. Solve the resulting linear least squares to find the step $\mathbf{d}^{\star}=\argmin_\mathbf{d}\left\|\mathbf{r}(\mathbf{x}^k)+\mathbf{J}_k\mathbf{d}\right\|^2$ by solving

   $$\left(\mathbf{J}_k^\top \mathbf{J}_k\right) \mathbf{d}=-\mathbf{J}_k^\top \mathbf{r}\left(\mathbf{x}^k\right)$$

   As discussed before, this is a linear system of equations, and should be solved by using Cholesky or QR decomposition rather than inverting $\mathbf{J}_k^\top\mathbf{J}_k$.

4. Update the current guess, $\mathbf{x}^{k+1}=\mathbf{x}^k+\mathbf{d}$.

Implementation example:

def gauss_newton(x0, max_iter, tol):
    x = x0.copy()
    for _ in range(max_iter):
        r = residual(x)
        J = jacobian(x)
        d = np.linalg.solve(J.T @ J, -J.T @ r) # Solve (J^T J) d = -J^T r
        x = x + d
        if np.linalg.norm(d) < tol:
            break
    return x

Gradient Descent

The steps of the gradient descent method are as follows:

1. Start from an initial guess $\mathbf{x}^0$, iterate until convergence $k=0, 1, 2, \dots$

2. Compute the gradient of the objective function

   $$\nabla f(\mathbf{x}^k)=\mathbf{J}_k^\top \mathbf{r}(\mathbf{x}^k)$$

   • Derivation

     Note that

     $$f(\mathbf{x})=\frac{1}{2}\left\|\mathbf{r}(\mathbf{x})\right\|^2=\frac{1}{2}\sum_{i=1}^{m}r_i^2\left(\mathbf{x}\right)^2$$

     and the $j$-th entry of $\nabla f$ is

     $$\frac{\partial f}{\partial x_j}=\sum_{i=1}^{m}r_i\left(\mathbf{x}\right)\frac{\partial r_i}{\partial x_j}=\mathbf{J}_{k,j}^\top\mathbf{r}(\mathbf{x})$$

     Then, stack all the entries of $\nabla f$ and we can get the result $\nabla f(\mathbf{x}^k)=\mathbf{J}_k^\top \mathbf{r}(\mathbf{x}^k)$.

3. Let the step $\mathbf{d}$ be the negative of the gradient, scaled by a factor called the learning rate $\eta$, i.e., $\mathbf{d}=-\eta \mathbf{J}_k^\top \mathbf{r}(\mathbf{x}^k)$.

4. Update the current guess, $\mathbf{x}^{k+1}=\mathbf{x}^k+\mathbf{d}$.

Implementation example:

def gradient_descent(x0, max_iter, tol, eta):
    x = x0.copy()
    for _ in range(max_iter):
        r = residual(x)
        J = jacobian(x)
        d = -eta * J.T @ r # Directly set d to be along the opposite gradient
        x = x + d
        if np.linalg.norm(d) < tol:
            break
    return x

Levenberg-Marquardt Method

The step of the Levenberg-Marquardt method is as follows:

1. Start from an initial guess $\mathbf{x}^0$, iterate until convergence $k=0, 1, 2, \dots$

2. Linearize the residual at the current guess $\mathbf{r}\left(\mathbf{x}^k+\mathbf{d}\right)=\mathbf{r}\left(\mathbf{x}^k\right)+\mathbf{J}_k\mathbf{d}$.

3. Solve a $\ell_2$ regularized linear least squares problem by $\mathbf{d}^\star=\argmin_{\mathbf{d}}\left\|\mathbf{r}\left(\mathbf{x}^k\right)+\mathbf{J}_k\mathbf{d}\right\|^2+\lambda \left\|\mathbf{d}\right\|^2$.

   $$\left(\mathbf{J}_k^\top \mathbf{J}_k+\lambda \mathbf{I}\right) \mathbf{d}=-\mathbf{J}_k^\top \mathbf{r}\left(\mathbf{x}^k\right)$$

   • Derivation

     Denote $\mathbf{r}_k\coloneqq \mathbf{r}\left(\mathbf{x}^k\right)$, the new objective function is

     $$\begin{align*} g(\mathbf{d}) &= (\mathbf{r}_k + \mathbf{J}_k \mathbf{d})^\top(\mathbf{r}_k + \mathbf{J}_k \mathbf{d}) + \lambda\,\mathbf{d}^\top\mathbf{d}\\ &= \mathbf{r}_k^\top\mathbf{r}_k + \mathbf{r}_k^\top\mathbf{J}_k\mathbf{d} + \mathbf{d}^\top\mathbf{J}_k^\top\mathbf{r}_k + \mathbf{d}^\top\mathbf{J}_k^\top\mathbf{J}_k\mathbf{d} + \lambda\mathbf{d}^\top\mathbf{d}\\ &= \mathbf{r}_k^\top\mathbf{r}_k + 2\,\mathbf{r}_k^\top\mathbf{J}_k\mathbf{d} + \mathbf{d}^\top\mathbf{J}_k^\top\mathbf{J}_k\mathbf{d} + \lambda\,\mathbf{d}^\top\mathbf{d} \end{align*}$$

     Take the gradient of $g$, and let $\nabla g(\mathbf{d})=\mathbf{0}$

     $$\begin{align*} \nabla g(\mathbf{d}) = 2\,\mathbf{J}_k^\top\mathbf{r}_k + 2\,\mathbf{J}_k^\top\mathbf{J}_k\,\mathbf{d} + 2\lambda\,\mathbf{d} = \mathbf{0} && \implies && \boxed{(\mathbf{J}_k^\top\mathbf{J}_k + \lambda\mathbf{I})\mathbf{d} = -\mathbf{J}_k^\top\mathbf{r}(\mathbf{x}^k)} \end{align*}$$

4. Update the current guess, $\mathbf{x}^{k+1}=\mathbf{x}^k+\mathbf{d}$.

Implementation example:

def levenberg_marquardt(x0, max_iter, tol, lam):
    x = x0.copy()
    for _ in range(max_iter):
        r = residual(x)
        J = jacobian(x)
        A = J.T @ J + lam * np.eye(len(x))
        d = np.linalg.solve(A, -J.T @ r) # Solve (J^T J + λI) d = -J^T r
        x = x + d
        if np.linalg.norm(d) < tol:
            break
    return x

Note that the Levenberg-Marquardt method is an interpolation between Gauss-Newton and gradient descent.

• As $\lambda\to 0$, it reduces to Gauss-Newton: $\left(\mathbf{J}_k^\top \mathbf{J}_k+\lambda \mathbf{I}\right) \to \mathbf{J}_k^\top \mathbf{J}_k$.

• As $\lambda \to \infty$, it reduces to gradient descent: $\left(\mathbf{J}_k^\top \mathbf{J}_k+\lambda \mathbf{I}\right) \to \lambda \mathbf{I}$.

Example

Now we compare these three methods using an example. Consider the residual and objective as

Besides state estimation or localization, which provide a robot with knowledge of where it is, it’s equally important for a mobile robot to perceive its surrounding and form a knowledge of where is occupied, that’s where mapping comes in. This note covers a fundamental mapping algorithm, occupancy grid mapping.

Problem Statement

In occupancy grid mapping, a map $\mathbf{m}=\left\{m_i\right\}_{i=1}^N$ is discretized into $N$ different cells, where each cell $m_i$ is a binary random variable that models the occupancy, and $p(m_i)$ is the probability of a cell being occupied. The map is assumed to be static. The goal is to find the belief $\mathrm{bel}_t(\mathbf{m})=p(\mathbf{m}\mid \mathbf{z}_{1:t}, \mathbf{x}_{1:t})$, where $\mathbf{z}$ is the observation and $\mathbf{x}$ is the state of the robot.

Two assumptions are made for tractability

• The robot state $\mathbf{x}_t$ are known.

• The occupancy of individual cells $m_i$ are independent, and thus

At each timestep $t$, the sensor performs some measurement and provide the information of whether a cell $m_i$ is believed to be occupied or not, in terms of $p(m_i\mid\mathbf{z}_t, \mathbf{x}_t)$, which is known as the inverse sensor model.

One simple inverse sensor model is shown in the figure above. Consider a laser beam range sensor that provides the reading $\mathbf{z}=(z_1^r,\dots, z_n^r,z_{1}^{\phi},\dots,z_{n}^{\phi})$, where $z_k^r$ and $z_k^\phi$ are the range and bearing of the $k$-th beam. For grid $i$, let $r_i$ and $\phi_i$ be the range and bearing of the grid as observed from the current robot pose $\mathbf{x}$, and $k=\argmin_j\left|\phi_i-z_j^{\phi}\right|$ be the beam going through that gird. Based on the relation of a grid’s position relative to the robot, we have three different cases:

• If $r_i>\min(z_{\max},z_k^r+w_{\mathrm{grid}})$ or $\left|\phi_i-z_{k}^{\phi}\right|>w_{\mathrm{beam}} / 2$, then return $p_{\mathrm{previous}}$, the sensor has no information about this grid.

• Else if $z_k^r < z_{\max}$ and $\left|r_i-z_k^r\right|<w_{\mathrm{grid}}$, then return $p_{\mathrm{occupied}}$, the sensor think the grid is occupied.

• Else if $z_k^r<z_{\max}$ and $r_i < z_k^r$, then return $p_{\mathrm{free}}$, the sensor think the grid is free.

where $z_{\max}$ is a sensor-specific parameter representing the maximum detectable range, $w_{\mathrm{grid}}$ is the grid size and $w_{\mathrm{beam}}$ is the width of each beam.

Mapping Algorithm

Following the Bayes filtering framework, a general algorithm can be derived for the occupancy grid mapping [2]. Starting from equation 1,

Further notice that the map $\mathbf{m}$ is independent of the robot state $\mathbf{x}_t$, we obtain

Similarly, for the negation

Compute the ratio of equation (2) and (3), we get

Then, change to log odds form by defining

and the product chain turns into a sum

Bayes Inference using Conjugate Prior

The previous section derived an equation for updating the belief of the map under the Bayes framework, however, it lacks the notion of uncertainty (i.e., how confident we are about the map). With the notion of conjugate prior, a proper selection of the underlying distribution function for $p(m_i)$ can both simplifies the updating rule and provide a closed form formula for uncertainty.

Recall that from Bayes rule

Bernoulli Likelihood and Beta Prior

First consider the case where the occupancy of a grid is binary, i.e., $m_i\in\{0, 1\}$. Let $\theta_i\triangleq p(m_i=1)$ be the unknown occupancy probability. From the inverse sensor model described above, we know that each sensor measurement provide a binary result, denoted as $y_t$, where

Then, the likelihood is modeled as a Bernoulli with parameter $\theta_i$, i.e.,

Furthermore, if we model the prior as a Beta distribution, i.e.,

where $B(\cdot, \cdot)$ is the beta function.

Then, it follows from the Bayes rule that the posterior is

which still follows a Beta distribution, $p(\theta_i\mid \mathbf{z}_{1:t},\mathbf{x}_{1:t})=\mathrm{Beta}(\theta_i;\alpha_t, \beta_t)$, where

In this case, we call beta distribution the conjugate prior for the Bernoulli likelihood.

• Conjugate Distribution and Conjugate Prior [2]

  • If the posterior distributions are in the same probability distribution family as the prior probability distribution, the prior and posterior are then called conjugate distributions.

  • The prior is called a conjugate prior for the likelihood function.

Note that the choice of modeling the prior as a Beta distribution here is an algebraic convenience, which provides a closed-form expression for the posterior. Then the mean and covariance can be easily obtained