Least squares problems and nonlinear optimization

0 Preface

Reprinted from here , minor errors corrected.

1. Least squares problem

When solving the optimal state estimation problem in SLAM, we generally get two variables. One is the actual observation value z obtained by the sensor \ $z$ , one is the predicted value h ( x ) h(\boldsymbol{x})calculated based on the currently estimated state quantity and observation model$h\left(x\right)$ . When solving the optimal state estimation problem, we usually try to minimize the square of the residual calculated from the predicted value and the observed value (the square is used to unify the influence of the sign), that is, to solve the following least squares problem:

$${\mathbf{x}}^{\ast }=\arg \underset{x}{\min }||\mathbf{z}-h(\mathbf{x})|{|}^{2If}$$
the observation model is a linear model, the above equation is transformed into a linear least squares problem:

$${\mathbf{x}}^{\ast }=\arg \underset{x}{\min }||\mathbf{z}-\mathbf{H}\mathbf{x}|{|}^{2For}$$
the linear least squares problem, we can directly find the closed-form solution:$x^{\ast }=-({\mathbf{H}}^T\mathbf{H}{)}^{-1}{\mathbf{H}}^{T}z$will not be described in detail here.

In actual problems, we usually have to minimize more than one residual. Different residuals are usually assigned a corresponding weight coefficient according to their importance (uncertainty), and the observation model is usually nonlinear, that is, solving the following problems:

$$\begin{array}{rl}{\mathbf{e}}_i(\mathbf{x})& ={\mathbf{z}}_i-h_i\left(x\right)i=1,2,...,n\\ ||{\mathbf{e}}_i(\mathbf{x})|{|}_{S_i}^2& =e_i^T{S}_i{e}_i\\ {\mathbf{x}}^{\ast }& =\arg \underset{\mathbf{x}}{\min }F(\mathbf{x})\\ & =\arg \underset{x}{\min }\sum _i||{\mathbf{e}}_i(\mathbf{x})|{|}_{S_i}^2\end{array}$$
We want to obtain a state quantity $x^{\ast }$ , such that the loss function$F\left(x\right)$ obtains a local minimum.

Before solving the specific problem, first consider $Properties\; of\; F\left(x\right)$ , perform a second-order Taylor expansion on it:

$$F(\mathbf{x}+\Delta x)=F(\mathbf{x})+\mathbf{J}\Delta \mathbf{x}+\frac{1}{2}\Delta x{\_}^T\mathbf{H}\Delta \mathbf{x}+O(||\Delta \mathbf{x}|{|}^3)$$
Ignoring higher-order remainders, the quadratic function has the following properties:

If at point ${\mathbf{x}}^{At\; \ast }$ , the derivative is$0$ , then this point is a stable point and has different properties according to the positive definiteness of the Hessian matrix:

• if $H$ is a positive definite matrix, then$F({\mathbf{x}}^{\ast })$is the local minimum
• if $H$ is a negative definite matrix, then$F\left(x^{\ast }\right)$is the local maximum
• if $H$ is an indefinite matrix, then$F\left(x^{\ast }\right)$is the saddle point

In the actual process, generally $F\left(x\right)$ is relatively complex. We have no way to directly make its derivative 0 and then find this point. Therefore, the iterative method is commonly used, that is, finding a descending direction so that the loss function changes with x \boldsymbol{x$The\; iterations\; of\; x$ gradually decrease until$x$ converges to${\mathbf{x}}^{\ast }$ . Here are some commonly used iterative methods:

2. Iterative descent method

As mentioned above, we need to find an x ​​\boldsymbol{x}The iteration amount of$x$$F\left(x\right)$ decreases. This process is divided into two steps:

Find $The\; descending\; direction\; of\; F\left(x\right)$ , construct the unit vector d in this direction$d$ determines the iteration step size α \alpha
in this directionIndependent variable x + α d \boldsymbol{x} + \alpha\boldsymbol{d}
after$\alpha \; iteration$$\mathbf{x}+The\; function\; value\; corresponding\; to\; \alpha d$ can be approximated by a first-order Taylor expansion (when the step size is small enough):

$$F(\mathbf{x}+\alpha \mathbf{d})=F(\mathbf{x})+\alpha Jd$$
Therefore, it is not difficult to find that to maintain$F\left(x\right)$ is decreasing, just ensure that$\mathbf{Jd}<0$ . The following methods all use different ideas to iterate in finding a suitable direction.

3. Steepest descent method

Based on the previous part, we know that the change is $\alpha Jd$ , where$Jd=||\mathbf{J}||\cos \theta$，$\theta$ is the gradient$J$ and$d$ angle. When$i=$When$-$$\pi$$Jd=-||J||$ obtains the minimum value. At this time, the direction vector is:

$$\mathbf{d}=\frac{-{\mathbf{J}}^T}{||\mathbf{J}||}$$
Therefore, along the gradient $The\; negative\; direction\; of\; J$ can make$F\left(x\right)$ , but in the actual process, this method is generally only used at the beginning of the iteration. When it is close to the optimal value, this method will oscillate and converge slowly.

4.Newton’s method

For F ( x ) F(\boldsymbol{x})The second-order Taylor expansion of$F$$($$x$$)\; is:$

$$F(\mathbf{x}+\Delta x)=F(\mathbf{x})+\mathbf{J}\Delta \mathbf{x}+\frac{1}{2}\Delta x{\_}^{T}H\Delta x$$
What we focus on is to find a$\Delta \mathbf{x}$ 使得 $J\Delta x+\frac{1}{2}\Delta x{\_}^{T}H\Delta x$is the smallest, so it can be derived:

$$\begin{array}{rl}\frac{\partial (\mathbf{J}\Delta \mathbf{x}+\frac{1}{2}\Delta x{\_}^T\mathbf{H}\Delta \mathbf{x})}{\partial \Delta \mathbf{x}}& ={\mathbf{J}}^T+\mathbf{H}\Delta \mathbf{x}=0\\ \Rightarrow \Delta & =-{\mathbf{H}}^{-1}{J}^T\end{array}$$
When $H$ is a positive definite matrix and the current$When\; x$ is near the optimal point, take$\Delta x\_=-{\mathbf{H}}^{-1}{J}^T$ can make the function obtain a local minimum. But the disadvantage is that the Hessian function of the residual is usually difficult to find.

5. Damping method

On the basis of Newton's method, in order to control each iteration not to be too aggressive, we can add a penalty term to the loss function, as shown below:

$$\arg \underset{\Delta x\_}{\min }\left\{F(\mathbf{x})+\mathbf{J}\Delta \mathbf{x}+\frac{1}{2}\Delta x{\_}^T\mathbf{H}\Delta \mathbf{x}+\frac{1}{2}m\left(\Delta x{)}^T\right(\Delta x)\right\}$$
When we choose$When\; \Delta x$ is too large, the loss function will also become larger, and the amplitude of the increase is given by$\mu$ is determined, so we can control the amount of each iteration$The\; size\; of$$\Delta x$ . Also in the right part forΔ x \Delta\boldsymbol{x}The derivative of$\Delta x$$is\; :$

$$\begin{array}{rl}{\mathbf{J}}^T+\mathbf{H}\Delta \mathbf{x}+mDx& =0\\ (\mathbf{H}+\mu \mathbf{I})\Delta \mathbf{x}=-{\mathbf{J}}^T& \end{array}$$
This idea will also be used when we introduce the LM method later.

6.Gauss-Newton (GN) method

In the previous arrangement, what we actually solved was the sum of a series of residuals. It is relatively simple to find the Jacobian of a single residual, so in the following methods we focus on the changes in each residual . Write each residual in the above nonlinear least squares problem in vector form:

$$\mathbf{F}(\mathbf{x})=E\left(x\right)=\left[\begin{array}{c}{\mathbf{e}}_1(\mathbf{x})\\ e_2\left(x\right)\\ ...\\ e_n(\mathbf{x})\end{array}\right]$$

For e ( x ) \boldsymbol{e}(\boldsymbol{x})Taylor expansion of$e$$($$x$$)$

$$and(x+\Delta x)=and\left(x\right)+J\Delta x$$
In the above formula,$J$ is the residual matrix$The\; Jacobian\; matrix\; of\; e\left(x\right)$versus state quantities.

Note that in the original linear least squares problem, there is also a weight matrix $\Sigma$ . In this case, we only need to let${\mathbf{e}}_i(\mathbf{x})=\sqrt{S_i}{e}_i\left(x\right)$ is enough. Therefore, Σ \boldsymbol{\Sigma}is not considered in the following formula.$The\; influence\; of\; \Sigma$ .

In this form, for e ( x ) \boldsymbol{e}(\boldsymbol{x})Taylor expansion of$e$$($$x$$)$

$$\begin{array}{rl}||e(\mathbf{x}+\Delta x)|{|}^2& =and(x+\Delta x{)}^{T}e(x+\Delta x)\\ & =\left(and\right(x)+\mathbf{J}\Delta \mathbf{x}{)}^T(\mathbf{e}(\mathbf{x})+J\Delta x)\\ & =and\left(x{)}^{T}e\right(x)+\Delta x{\_}^T{\mathbf{J}}^{T}e(x)+and(x{)}^T\mathbf{J}\Delta \mathbf{x}+\Delta x{\_}^T{J}^T\mathbf{J}\Delta \mathbf{x}\end{array}$$
In the above formula, note: $e\left(x\right)$ is one-dimensional, so$\Delta x{\_}^T{\mathbf{J}}^{T}e\left(x\right)=and(x{)}^{T}J\Delta x$, so simplify to:

$$\begin{array}{rl}F(\mathbf{x}+\Delta x)& =and\left(x{)}^{T}e\right(x)+2\mathbf{e}(\mathbf{x}{)}^T\mathbf{J}\Delta \mathbf{x}+\Delta x{\_}^T{\mathbf{J}}^{T}J\Delta x\\ & =F(\mathbf{x})+2\mathbf{e}(\mathbf{x}{)}^{T}J\Delta x+\Delta x{\_}^T{J}^T\mathbf{J}\Delta \mathbf{x}\end{array}$$
In this way, we also approximate it as a quadratic function, and compared with our previous expansion results, it is not difficult to find that here we actually use ${\mathbf{J}}^{T}e$to approximate the Jacobian matrix, use$J^{T}J$to approximate the Hessian matrix. Therefore, when$When\; J$ is of full rank, we can ensure that the function obtains a local minimum where the derivative of the above formula is 0. Similarly, on the right side of the above formula, forΔ x \Delta\boldsymbol{x}Derivating$\Delta$$x\; and\; making\; it\; equal\; to\; 0\; is:$

$$\begin{array}{rl}{\mathbf{J}}^{T}e\left(x\right)+J^T\mathbf{J}\Delta \mathbf{x}=0& \\ \Rightarrow J^{T}J\Delta x& =-{\mathbf{J}}^{T}e\left(x\right)\\ \Rightarrow \mathbf{H}\Delta \mathbf{x}& =\mathbf{b}\end{array}$$
In the above formula, we let $\mathbf{H}=J^T\mathbf{J},\mathbf{b}=-J^{Te}$ .$\_$In this way we obtain the solution process of Gauss-Newton method:

• Calculate the Jacobian matrix J of the residual matrix with respect to the state value $\mathbf{J}$
• Use the Jacobian matrix and residuals to construct the information matrix and information vector $\mathbf{H},\mathbf{b}$
• Calculate the current iteration amount: $\Delta x\_={\mathbf{H}}^{-1}\mathbf{b}$
• If the iteration amount is small enough, end the iteration, otherwise enter the next iteration

7. Levenberg-Marquardt (LM) method

The LM method is based on the Gauss-Newton method and adds a damping factor according to the idea of ​​the damping method, that is, solving the following equation:

$$(\mathbf{H}+\mu \mathbf{I})\Delta \mathbf{x}=bIn$$
the above formula, the functions of the damping factor are:

• Add to $H$ guarantees$H$ is positive definite

• 当$\mu$ 很大时，$\Delta x\_=-(\mathbf{H}+\mu I{)}^{-1}\mathbf{b}\approx -\frac{1}{m}\mathbf{b}=-\frac{1}{m}{\mathbf{J}}^{T}E(x)$, close to the steepest descent method

• 当$When\; \mu$ is very small, it is close to the Gauss-Newton method.
  Therefore, by setting the damping factor reasonably, the iteration speed can be dynamically adjusted. The setting of damping factor is divided into two parts:

• Selection of initial values

• Update strategy that changes with iteration volume

Let’s first look at the initial value selection method. The size of the damping factor should be based on $J^{}$To select the size of${}^T$$J$$J^{T}J$performs eigenvalue decomposition, there are:${\mathbf{J}}^T\mathbf{J}=\mathbf{V}\mathbf{\Lambda }{\mathbf{V}}^T$，$L=\text{diag}({\lambda }_1,l_2,...,l_n),\mathbf{V}=[{\mathbf{v}}_1,...,{\mathbf{v}}_n]$ , therefore, the iteration formula is simplified to:

$$\begin{array}{rl}(\mathbf{V}\mathbf{\Lambda }{\mathbf{V}}^T+\mu \mathbf{I})\Delta \mathbf{x}& =\mathbf{b}\\ \Delta x\_& =(V\Lambda V^T+\mu I{)}^{-1}\mathbf{b}\\ & =-\sum _i\frac{{\mathbf{v}}_i^T\mathbf{b}}{l_i+m}{v}_i\end{array}$$
Therefore, $\mu$ selection$l_i$It can be close. A simple idea is to set $m_0=t\max ({\mathbf{J}}^T\mathbf{J}{)}_{ii}$, in practice it is generally assumed that $t\approx [10^{-8},1]$

Next look at $For\; the\; update\; strategy\; of\; \mu$ , first qualitatively analyze how to update the damping factor:

• 当 $\Delta x$ let$When\; F\left(x\right)$ increases,$\mu$ to reduce$\Delta x\; reduces\; the\; impactof$ this iteration by reducing the step size
• 当 $\Delta x$ let$When\; F\left(x\right)$ decreases,$\mu$ to increase$\Delta x$ increases the impact of this iteration by increasing the step$size$

Let's conduct quantitative analysis below, let $L(\Delta \mathbf{x})=F(\mathbf{x})+2\mathbf{E}(\mathbf{x}{)}^T\mathbf{J}\Delta \mathbf{x}+\frac{1}{2}\Delta x{\_}^T{\mathbf{J}}^{T}J\Delta x$takes into account the following scaling factors:

$$r=\frac{F(\mathbf{x})-F(\mathbf{x}+\Delta x}{L(0)-F(\Delta \mathbf{x})}$$
Marquardt proposes a strategy:

• 当 $r<0$ , indicating the current$\Delta x$ makes$F\left(x\right)$ increases, indicating that it is still far from the optimal value, and$\mu$ makes it close to the steepest descent method for larger updates
• When $r>0$ and relatively large, indicating the current$\Delta x$ makes$F\left(x\right)$ decreases,$\mu$ makes it close to the Gauss-Newton method, reducing the speed and updating it to the optimal point
• if $r>0$ but relatively small, indicating that it has reached near the optimal point, then increase the damping$\mu$ , reduce the iteration step size

Marquardt’s specific strategy is as follows:

if rho < 0.25: 
    mu = mu * 2
else if rho > 0.75: 
    mu = mu /3

An update process using the Marquardt strategy is as follows:

It can be found that the effect is not very good. As the number of iterations increases, $\mu$ begins to oscillate, indicating that the iteration amount periodically changes$F\left(x\right)$ increases and then decreases.

Therefore, Nielsen proposed another strategy, also used in G2O and Ceres:

if rho > 0:
    mu = mu * max(1/3, 1 - (2 * rho - 1)^3)
    v = 2
else:
    mu = mu * v
    v = 2 * v

An example of optimization using this strategy is as follows:

It can be seen that $\mu$ can continue to decrease relatively smoothly as the iteration proceeds until convergence is reached.

8. Robust kernel function

When performing least squares problems, we will encounter some abnormal observation values ​​that make the observation residuals particularly large. If these abnormal points are not processed, it will affect the optimization process. The optimizer will try to minimize the abnormal residual terms. Finally, Affecting the accuracy of state estimation, the robust kernel function is used to reduce the impact of these abnormal observations.

Apply the robust kernel function directly to each residual term to transform the least squares problem into the following form:

$$F(\mathbf{x})=\sum _i\rho (||e_i(\mathbf{x})|{|}^2)$$
The process of solving nonlinear least squares when using the robust kernel function.
In this form, perform a second-order Taylor expansion on the residual with the robust kernel function:

$$p(s+\Delta s)=p\left(x\right)+r^{\prime }(x)\Delta s+\frac{1}{2}{r}^{"}\left(x\right)D^{2}s$$
In the above formula, the change amount$\Delta sis$ calculated as follows:

$$\begin{array}{rl}\Delta s{\_}_k& =||e_i(\mathbf{x}+\Delta x)|{|}^2-||e_i(\mathbf{x})|{|}^2\\ & =||e_i(\mathbf{x})+{\mathbf{J}}_i\Delta \mathbf{x}|{|}^2-||e_i(x)|{|}^2\\ & =2e{\_}_i(x{)}^T{\mathbf{J}}_i\Delta x\_+\Delta x{\_}^T{J}_i^T{J}_i\Delta \end{array}$$
将$\Delta s$代入$p(s+\Delta s)$ can be obtained:

$$\begin{array}{rl}p(s+\Delta s)=& p\left(s\right)+r^{\prime }\left(s\right)(2e_i(\mathbf{x}{)}^T{\mathbf{J}}_i\Delta x\_+\Delta x{\_}^T{J}_i^T{\mathbf{J}}_i\Delta x)\\ & +\frac{1}{2}{r}^{\prime \prime }\left(s\right)\left(2e_i\right(x{)}^T{J}_i\Delta x\_+\Delta x{\_}^T{J}_i^T{J}_i\Delta x{)}^2\\ \approx & p\left(s\right)+2p^{\prime }(s)e_i(x{)}^T{J}_i\Delta x\_+r^{\prime }(s)\Delta {\mathbf{x}}^T{J}_i^T{J}_i\Delta x\_\\ & +2p^{\prime \prime }(s)\Delta {\mathbf{x}}^T{J}_i^T{e}_i\left(x\right)e_i(x{)}^T{J}_i\Delta \end{array}$$
According to the previous idea, take the derivative of the above formula and make it equal to 0, we can get:

$$\sum _i{\mathbf{J}}_i^T(r^{\prime }(s)+2p^{\prime \prime }(s)e_i(x)e_i(\mathbf{x}{)}^T)\mathbf{J}\Delta \mathbf{x}=-\sum _i{r}^{\prime }(s){\mathbf{J}}_i^T{e}_i(x)$$
Compare the previous matrix${\mathbf{J}}^T\mathbf{J}\Delta \mathbf{x}=-{\mathbf{J}}_i^{T}e\left(x\right)$can be obtained. After we use the robust kernel function, we only need to calculate the first-order and second-order derivatives of the kernel function of each residual, and then update the information matrix and information vector according to the above form. Can.

Commonly used robust kernel functions
Cauchy robust kernel function:

$$\begin{array}{rl}p\left(s\right)& =c^2\log (1+\frac{s}{c^2})\\ r^{\prime }(s)& =\frac{1}{1+\frac{s}{c^2}}\\ r^{\prime \prime }(s)& =-\frac{1}{c^2}(r^{\prime }(s){)}^2\end{array}$$
Among them, $c$ is the control parameter. When the residuals are normally distributed, Huber c is selected as 1.345 and Cauchy c is selected as 2.3849. The effects of different robust kernel functions are shown in the figure below: