總結一下SLAM中關於非線性優化的知識。
先列出參考：
http://jacoxu.com/jacobian%E7%9F%A9%E9%98%B5%E5%92%8Chessian%E7%9F%A9%E9%98%B5/
http://blog.csdn.net/dsbatigol/article/details/12448627
http://www.cnblogs.com/rongyilin/archive/2012/12/21/2827898.html
《視覺SLAM十四講》。
1. 雅克比矩陣 && 海森矩陣
雅克比矩陣（Jacobian）是一階偏導數以一定方式排列成的矩陣，其行列式稱為雅克比行列式。它體現了一個可微方程與給出點的最優線性逼近，類似與多元函數的導數。
假設F：Rn→Rm是一個從歐式n維空間轉換到歐式m維空間的函數，這個函數由m個實函數組成：u1（x1，...，xn），...，um（x1，...，xn）。假設這些函數的偏導數存在，則雅克比矩陣可以寫成：
海森矩陣（Hessian）是一個自變量為向量的實值函數的二階偏導數組成的方塊矩陣，設函數為：f（x1，x2，...，xn），並假設f的二階導數粗在，則其海森矩陣為：
海森矩陣常被應用於牛頓法解決大規模的優化問題，具體可參考：http://jacoxu.com/jacobian%E7%9F%A9%E9%98%B5%E5%92%8Chessian%E7%9F%A9%E9%98%B5/。
2.最小二乘問題
最小二乘法（又稱最小平方法）是一種數學優化技術。它通過最小化誤差的平方和尋找數據的最佳函數匹配。
2.1 線性最小二乘
已知M個N+1維空間點：
，
其中，
。
如何求的函數f(x)：
。
可以構建最小二乘問題估計f(x)的參數：
，
寫成矩陣形式為：
，
其中：
為了最小化上式，對w求導，並另導數等於0：
。
上式的獲取需要先展開平方項，然后進行矩陣的求導。只要求出w矩陣就可以了，一般線性最小二乘需要用到Cholesky的分解，具體的求解過程就不展開了。
2.2 非線性最小的二乘
非線性最小二乘指的是f(x)函數是一個非線性函數，導致導數的求解變的非常困難，因此需要求解非線性最小二乘問題的方法。而求解方法中就包括最速下降法，牛頓法，高斯-牛頓法(G-N)，列文伯格-馬誇爾特法(L-M)等，它們都是通過迭代的方式逼近最優解，下面分別做一個簡單的介紹。
3. 1最速下降法
它的思想是每次迭代選取一個步長λ，根據函數的梯度選擇下降最快的方向，使得目標函數的值能夠最大程度的下降。這里主要是理解算法思想，具體的算法操作還應該查閱相關書籍。我們假設f(x)是一個待優化的函數（其實對於最小二乘來說，它應該是一個殘差函數，有的地方也叫cost founction）:min1/2||f(x)||2，對其進行一階泰勒展開：
其中的J(x)為雅克比矩陣，即函數平方關於x的導數，根據上式可以求取增量和步長λ，然后進入迭代循環。但是該方法收斂速度比較慢，直接使用最速下降法效果不好。
3.2 牛頓法
牛頓法相比最速下降法更進一步，對待優化函數進行二階泰勒展開：
則增量為：
它是一個關於雅克比矩陣和海森矩陣的問題，所以計算量稍大。
3.3 高斯-牛頓法
可以看到上面的方法都是對殘差函數的平方項直接進行泰勒展開，而高斯牛頓法則是對f(x)進行泰勒展開，然后對展開項進行平方和：
由於每次迭代都需要求解一個增量，現在可以構建關於增量的線性最小二乘問題：
因此只要對上式求導，並另導數為0，就能求解增量：
通常上式都寫成如下形式：
獲得這個增量方程，需要對上面的線性最小二乘問題進行矩陣的平方項計算以及相應的矩陣求導，其實這和前面說過的非線性最小二乘問題本質上是一樣的，獲得的數學結論也類似，但是關於上式的求解方法在SLAM系統里跟線性方法不太一樣。
3.4 列文伯格-馬誇爾特法
L-M法是對高斯牛頓法的一個改進，給增量添加一個信賴區域：
分子是實際函數的下降值，分母是近似模型的下降值（分母的問題可以回憶一下微積分）。上式太小，說明實際下降值小於近視下降值，所以你的估計下降的太快了；如果上式太大，說明實際下降值大於近似下降值，所以你可以快點下降沒關系。這樣，高斯牛頓法的最小二乘問題變成：
用拉格朗日乘數法就是：
同樣進行平方項的展開以及對增量的求導：
這就是L-M中的增量方程。