本篇博客為系列博客第二篇，主要介紹非線性最小二乘相關內容，線性最小二乘介紹請參見SLAM中的優化理論（一）—— 線性最小二乘。本篇博客期望通過下降法和信任區域法引出高斯牛頓和LM兩種常用的非線性優化方法。博客中主要內容為：
- 非線性最小二乘介紹；
- 下降法相關理論(Desent Method);
- 信任區域理論（Trust Region Methods）;
- 非線性最小二乘求解方法（高斯牛頓、LM）
由於個人水平有限，文中難免有解釋不清晰的地方，因此希望大家結合着[1]、[2]和[3]進行理解。如果在閱讀過程中發現有任何錯誤煩請告知。
如果有人知道如何將word中編輯的公式直接復制到博客上也麻煩給我說一下。
1. 非線性最小二乘介紹
1.1. 最小二乘問題回顧：
在上一篇博客中我們知道最小二乘問題是為了找到測量值和模型預測值之間的最小誤差，該問題可以簡單的表示為：
其中e(x)為模型和樣本之間的誤差，在SLAM中亦可以看作為觀測值和估計值之間的誤差。通過求解該問題我們就能夠優化我們的模型函數使之更接近與真實的函數模型。
下圖的數據擬合可以很好的說明最小二乘問題的結果和效果。
1.2. 線性與非線性最小二乘
假如該問題為線性的我們可以直接對目標函數求導，並且令其等於零，以此求得其極值，並通過比較求取全局最小值(Global Minimizer)，並將其最為目標函數的解。
但是如果問題為非線性，此時我們通常無法直接寫出其導數形式（函數過於復雜），因此我們不再去試圖直接找到全局最小值，而是退而求其次通過不停的迭代計算尋找到函數的局部最小值(Local Minimizer)，並認為該局部最小值能夠使得我們的目標函數取得最優解（最小值），這就是非線性最小二乘的通常求解思路。
1.3. 非線性最小二乘相關定義
我們可以定義非線性最小二乘問題形式為：
同時我們定義局部最小值滿足以下定義：
可理解為局部最小值的搜索區域，對於每一次的迭代，局部最小值與當前迭代的初始值之差不應超出設定的搜索區域。
現在我們將代價函數在點處進行泰勒展開有：
假設x為局部最小值，即使足夠小，我們也將無法找到一個點x+△x，使得其對應的函數值小於x處的函數值。因此我們也可以得到以下結論：
同時我們也將所有滿足令函數在其處一階導等於零的點為駐點(Stationary Point)。
綜上所述，我們知道局部最小值為函數的駐點，但是駐點同時也有肯能為函數的局部最大值或者鞍點(saddle point)。因此為區分某一個點到底是局部最小值、局部最大值或者是鞍點，我們在需要保留泰勒展開式的二階信息。通過判定展開式中的H(Hessian)矩陣的相關性質進行判定：
現在我們已經對非線性最小二乘問題的相關概念定義有了基本了解，因此接下來介紹兩種通用的求解思想和相關算法。
2. 下降法
幾乎所有求解非線性優化的方法都是從一個函數的初始值(
)開始求解得到一些列的值(
)，並希望最后函數收斂於局部最小值(
)。因此，我們能夠得到函數的下降條件為：
該條件可以使得函數最后不收斂於局部最大值，並且可以再一定程度上使得函數盡可能小概率的收斂於鞍點。
不過在實際操作當中，函數最后收斂於哪個局部最小值在很大程度上是取決於我們的初始值的，下圖可以直觀的表示這種影響。這也是在SLAM中為什么我們期望盡可能為后端優化得到一個准確的初始值的原因。不過因為本篇博客是為了講解優化方法的因此並不會深入討論如何獲取到精確的初始值。
2.1. 下降法計算流程
下降法滿足上面所提到的公式2.1中的條件，且每一步迭代中都主要包括兩部分內容，分別是求解下降方向
和合理的步長
。
因此下降法的整體流程如下，參考[1]。
該流程從給定的初始值開始，在每一步迭代中首先計算下降的方向，然后在計算合理的步長，步長計算采用line search方法。最后根據下降方向和步長產生△x，並與此次迭代的初始值x相加得到下次迭代的初始值
。以此類推直到最后找到收斂值后，停止計算。
為了進一步說明該方法的相關內容我們參考公式1.2對函數進行一階泰勒展開：
在此處我們將公式1.2中的△x拆解為和分別代表函數的下降步長和方向。
2.2. 下降方向
因為h為函數的下降方向，因此如果函數
是一個關於的下降函數，且
，那么我們就能得到如下定義：
即我們可以發現，函數的下降方向將永遠為函數的負梯度方向。該結論亦可通過下面兩張圖進行直觀的說明。
如上圖所示，如果當前迭代點在全局最小值左側，函數的梯度方向為y正x負，但是實際求解出來的下降方向為y負x正，正好與梯度方向相反。迭代點在全局最小值右側的情況也是一樣的。因此可以證明定義3。
2.3. 下降步長
現在我們已經能夠明確下降的方向h了，接着我們需要去計算下降的步長。下降的步長可以告訴在此次迭代中我們應該沿着下降方向h走多遠使得函數的值降低到一個合適的值。下降的步長計算方法有很多其中一種為line search方法， 具體形式為：
該公式是指在已知下降方向h后，我們通過求取函數F的最小值來反向求解出此次迭代的步長值，此處
為最后求解得到的步長值，且該值必須大於0。
Line Search方法的基本思想可以解釋為每一次都會去嘗試一個步長，並使用該步長沿着下降方向走，並檢測函數值是否會下降到一定程度。如果沒有，則按比例減小，並重復之前步驟知道滿足條件。
由於line search方法存在計算量大等一些問題在SLAM中其實運用的並不是太多（牛頓法和高斯牛頓法直接默認步長為1），因此在此處不再對line search方法進行進一步探究。
2.4. 最速下降法
前面已經對下降法的內容進行了介紹，了解這些概念后再來看看最速下降法算法就會比較好理解。
最速下降法與前面所述的下降法最大的不同就是其下降方向為：
該結論是通過相對增益方程推導出來的，即對於一個下降方向為h步長為的函數，其相對增益函數為：
角度theta為h和F一階導之間的向量夾角。如果theta=pi我們就能夠公式2.4所示的最快下降方向了。
然后在通過line search方法計算出來步長就能夠得到某一次迭代的更新增量△x，從而也能夠得到更新后的x值。之后再重復以上步驟不停迭代知道滿足條件收斂。
最速下降法雖然每次都是找的局部的最優下降方向，但是其快速性只能在局部中體現出來，從全局來看其計算速度反而比較慢，因此最速下降法在實際的SLAM計算中也並不會使用。不過了解該方法和背后的理論可以幫助我們理解后面其他算法的相關內容。
3. 信任區域法 (Trust Region Methods)
假設我們有一個模型L，在當前迭代點x附近，該模型可以被近似為函數F形式的二階泰勒展開式，則我們有：
此處我們用h替代變量，即當前迭代中需要在初始x上增加的量。
但是這個近似假設的成立是有一定程度限制的，我們可以設立一個正值變量△使得模型在一個以x為圓心為半徑的圓中被視為可以精確近似。這個圓就是我們所說的信任區域。然后我們就可以基於以下公式求解變化值h：
該公式表示，在信任區域半徑限制內，求解出最優的變化值h是的模型L的值最小。
但是在計算過程中，可信任區域半徑值是會不停改變的，其改變的依據為模型的近似質量，模型的近似質量又可以通過增益比例（gain ration）進行判定。
首先我們定義增益比例的概念。增益比例為實際函數改變量和模型該變量之間的差異，數學表達式為：
當我們得到增益比例后我們就可以通過它去實時控制可信任區域半徑以及迭代的步長：
如果增益比例過小（例如小於0.25），說明實際改變的量遠遠小於近似模型的值，此時近似情況並不理想，因此我們需要縮小近似范圍。反之如果比例較大，說明實際下降的比預期的更大，近似情況比較好，我們可以適當放大一下近似范圍。
以上就是信任區域法的簡單介紹，其具體應用和計算我們將會在下文中的L-M算法中進行介紹。
4. 非線性最小二乘問題計算
為了后面表述方便，首先我們再簡單回顧一下非線性最小二乘問題，並對問題中變量符號進行定義。非線性最小二乘問題目的為求解代價函數的最優解（局部最小值）使得函數的值最小：
上面公式中，函數
為我們需要求解的代價函數（目標函數），f(x)是由模型函數與測量值之差構成，
為代價函數的局部最小值也是我們最終需要求解的值。
4.1. 高斯牛頓法
高斯牛頓法是一種求解非線性最小二乘的簡單算法，該算法的基本思想是將函數非線性F(x)進行一階泰勒展開（此處我們展開的只是函數F(x)而非代價函數）：
此處J(x)為函數的雅克比矩陣，即函數F的一階導。
根據前面所述我們的目的其實就是求解合適的變量值△x使得函數的值
達到最小。
為了實現這個目的我們可以依照前面的框架構建一個最小二乘問題，即：
現在我們將通過x求解函數F(x)的最小值問題轉換為了通過△x求解函數F(x+△x)最小值的問題，同時目標函數也轉換為
。
現在我們將目標函數展開有：
為了求解其極值我們需要對式4.5求導並令其等於零。
由公式4.7我們將能夠得到一個關於△x的線性方程組，我們稱這個方程為高斯牛頓方程。如果我們將J(x)看做為A將F(x)看做為b，函數也可以寫成為：
我們會發現公式4.8與上一篇博客中所講的最小二乘的正規方程解一模一樣，因此公式4.7也被成為正規方程。
同時更進一步，因為高斯牛頓法其實是第二章中下降法的一種，為了明白兩者間的關系，我們可以講增量拆分為下降方向h和下降步長. 然后我們從公式4.7能夠發現，該公式的下降方向滿足下降法的約束，即沿着函數梯度的負方向，而步長則等於為1.
明白公式4.7的相關意義后，我們將其進行簡化：
此處的H矩陣其實是Hessian矩陣的近似，通過雅克比矩陣去避免掉真是的Hessian矩陣的計算。
基於以上內容我們能夠得到高斯牛頓算法的執行步驟，參考[2]：
在整個高斯牛頓算法中，增量方程即方程4.9的求解是最為主要的。根據本文前面所提到的，我們為了保證所求解的x為函數的局部最小值，我們需要保證H矩陣為正定的，但是在實際中H矩陣很有可能是半正定的或者其他情況，因此可能會發生算法不收斂或結果不准確的情況。
因為高斯牛頓算法有以上這些缺點因此在SLAM的實際優化問題中我們更多的是使用后文提到的LM算法。
4.2. Levenberg-Marquardt Method
LM算法大致與高斯牛頓算法理論相同，但是不同於高斯牛頓算法，LM算法是基於信賴區域理論(Trust Region Method)進行計算的。這是因為高斯牛頓法中的泰勒展開只有在展開點附近才會有比較好的效果，因此為了確保近似的准確性我們需要設定一個具有一定半徑的區域作為信賴區域（和第三節中的信賴區域理論道理相同）。
同第三節中所述相同，采用信賴區域法我們就需要明確該區域該怎么確定。在LM算法中信賴區大小的確定也是運用增益比例來進行判定的(參考公式3.3)：
基於信賴區域我們能夠重新構建一個更有效的優化框架，參考[2]。
我們引入拉格朗日乘子將該有約束優化問題轉化為無約束優化問題后我們能夠得到：
因為D=I，又有：
公式4.13就是LM算法的增量表達式。通過該式計算就可以避免掉高斯牛頓算法中出現的不收斂等問題。
參考內容:
[1]. Methods for Non-linear Least Squares Problems
[2]. 視覺SLAM十四講
[3]. Multiple View Geometry in Computer Vision, Appendix 6, Iterative Estimation Methods
[4]. 機器學習.Lecture 2（吳恩達）