预处理共轭梯度法(PCG)

开始读研了，研究方向是SLAM。SLAM中很重要的一部分是位姿估计，需要用到一些优化算法对位姿进行优化。预处理共轭梯度法PCG(Preconditioned Conjugate Gradient)是一种SLAM中常用的位姿优化算法，从共轭梯度法（Conjugate Gradient）衍生而来，用于快速计算最优值。

直接求解

---

假设有一个线性方程：
$$\text{A}\text{x}=\text{b}$$

其中$A$是一个已知的对称且正定的矩阵，b也已知。记${\text{x}}_{\ast }$为方程的唯一解。如果两个非零向量$\text{u,v}$满足：
$${\text{u}}^{\mathrm{T}}\text{A}\text{v}=0$$

则称$\text{u}$和$\text{v}$共轭（相对于$\text{A}$)。共轭是一种对称的关系，如果$\text{u}$对于$\text{v}$共轭，那么$\text{v}$对于$\text{u}$共轭。
由于$\text{A}$对称且正定，于是可以定义內积：
$$⟨\text{u},\text{v}{⟩}_{\text{A}}:=⟨\text{A}\text{u},\text{v}⟩=⟨\text{u},{\text{A}}^{\mathrm{T}}\text{v}⟩=⟨\text{u},\text{A}\text{v}⟩={\text{u}}^{\mathrm{T}}\text{A}\text{v}$$

如果有一个矩阵$\text{P}$：
P = { P 1 , P 2 , . . . . , P n } \textbf{P} = \left\{\textbf{P}_{1},\textbf{P}_{2},....,\textbf{P}_{n}\right\} P={P1​,P2​,....,Pn​}

且$\text{P}$中的列向量两两共轭，那么$\text{Ax}=\text{b}$的解${\text{x}}_{\ast }$可以写成：
$${\text{x}}_{\ast }=\sum _{i=1}^n{\alpha }_i{\text{P}}_i$$

于是，
$$\text{A}{\text{x}}_{\ast }=\sum _{i=1}^n{\alpha }_i\text{A}{\text{P}}_i$$

左乘${\text{P}}_k^{\mathrm{T}}$:
$${\text{P}}_k^{\mathrm{T}}\text{A}{\text{x}}_{\ast }=\sum _{i=1}^n{\alpha }_i{\text{P}}_k^{\mathrm{T}}\text{A}{\text{P}}_i$$

将$\text{A}\text{x}=\text{b}$和${\text{u}}^{\mathrm{T}}\text{A}\text{v}=⟨\text{u},\text{v}{⟩}_{\text{A}}$代入得：
$${\text{P}}_k^{\mathrm{T}}\text{b}=\sum _{i=1}^n{\alpha }_i⟨{\text{P}}_k，{\text{P}}_i{⟩}_{\text{A}}$$

因为${\text{u}}^{\mathrm{T}}\text{v}=⟨\text{u},\text{v}⟩$,且对于$\forall i̸=k:⟨\text{u},\text{v}{⟩}_{\text{A}}=0$,所以：

$⟨{\text{P}}_k,\text{b}⟩={\alpha }_k⟨{\text{P}}_k,{\text{P}}_k{⟩}_{\text{A}}$

由此可以得到${\alpha }_k$的表达式：
$${\alpha }_k=\frac{⟨{\text{b}}_k,\text{b}⟩}{⟨{\text{P}}_k,{\text{P}}_k{⟩}_{\text{A}}}$$

结合${\text{x}}_{\ast }=\sum _{i=1}^n{\alpha }_i\text{A}{\text{P}}_i$就可以求解${\text{x}}_{\ast }$。
当$n$很大时，用直接法求解就会非常耗时，于是引出迭代法。

迭代法

---

为了求得${\text{x}}_{\ast }$的一个很好的近似，我们并不需要全部的${\text{P}}_k$，当$n$很大且$\text{A}$又具有一定的稀疏性时，可以用迭代法来求得一个近似的结果。令${\text{x}}_{\ast }$的初始估计值为${\text{x}}_0$(${\text{x}}_0=\text{0}$,etc.)。
构建二次函数：

$$f(x)=\frac{1}{2}{\text{x}}^{\mathrm{T}}\text{A}\text{x}-{\text{x}}^{\mathrm{T}}\text{b}$$

该函数一、二阶导为:

$$Df(x)=\text{A}\text{x}-\text{b}，$$

$$D^{2}f(x)=\text{A},$$

由于$\text{A}$对称且正定，$f(x)$有唯一最小值。取${\text{P}}_0$为$f(x)$在${\text{x}}_0$处的负梯度，即${\text{P}}_0=\text{b}-{\text{Ax}}_0$，${\text{P}}_0$同时也是算法初始步骤的残差项。
令${\text{r}}_k$为第$k$步的残差：
$${\text{r}}_k=\text{b}-{\text{Ax}}_k$$

${\text{r}}_k$也是梯度下降法中的下降方向，在共轭梯度法中，为保证当前下降方向与之前步骤中的下降方向共轭，取：
$${\text{P}}_k={\text{r}}_k-\sum _{i<k}\frac{{\text{P}}_i^{\mathrm{T}}\text{A}{\text{r}}_k}{{\text{P}}_i^{\mathrm{T}}\text{A}{\text{P}}_i}{\text{P}}_i$$

沿着这个方向，更新后的$\text{x}$值为：
$${\text{x}}_{k+1}={\text{x}}_k+{\alpha }_k{\text{P}}_k$$

其中
$${\alpha }_k=\frac{{\text{P}}_k^{\mathrm{T}}\left(\text{b}-{\text{Ax}}_k\right)}{{\text{P}}_k^{\mathrm{T}}\text{A}{\text{P}}_k}=\frac{{\text{P}}_k^{\mathrm{T}}{\text{r}}_k}{{\text{P}}_k^{\mathrm{T}}\text{A}{\text{P}}_k}$$

${\alpha }_k$通过最小化下式得到，
$$f({\text{x}}_{k+1})=f({\text{x}}_k+{\alpha }_k{\text{P}}_k)=:g({\alpha }_k)$$
令其对${\alpha }_k$的导数为0。
$$g^{\prime }({\alpha }_k)\stackrel{!}{=}0\iff {\alpha }_k=\frac{{\text{P}}_k^{\mathrm{T}}(\text{b}-{\text{Ax}}_k)}{{\text{P}}_k^{\mathrm{T}}{\text{AP}}_k}$$

共轭梯度法流程

---

${\text{r}}_0:=\text{b}-{\text{Ax}}_0$
${\text{P}}_0:={\text{r}}_0$
$k:=0$
repeat
${\alpha }_k:=\frac{{\text{r}}_k^{\mathrm{T}}{\text{r}}_k}{{\text{P}}_k^{\mathrm{T}}{\text{AP}}_k}$
${\text{x}}_{k+1}:={\text{x}}_k+{\alpha }_k{\text{P}}_k$
${\text{r}}_{k+1}:={\text{r}}_k-{\alpha }_k\text{A}{\text{P}}_k$
if ${\text{r}}_{k+1}$ is suffiently small ,then exit loop.
${\beta }_k:=\frac{{\text{r}}_{k+1}^{\mathrm{T}}{\text{r}}_{k+1}}{{\text{r}}_k^{\mathrm{T}}{\text{r}}_k}$
${\text{P}}_{k+1}:={\text{r}}_{k+1}+{\beta }_k{\text{P}}_k$
$k:=k+1$
end repeat

共轭梯度法代码

function [x] = conjgrad(A, b, x)
    r = b - A * x;
    p = r;
    rsold = r' * r;

    for i = 1:length(b)
        Ap = A * p;
        alpha = rsold / (p' * Ap);
        x = x + alpha * p;
        r = r - alpha * Ap;
        rsnew = r' * r;
        if sqrt(rsnew) < 1e-10
              break;
        end
        p = r + (rsnew / rsold) * p;
        rsold = rsnew;
    end
end

没写完，待续、