理解slam中的非線性優化問題

主要參考了高翔老師的14講中的知識，以及結合imu預積分**（on-manifold preintegration for real-time visual-inertial odometry），結合自己的理解，做了一下整理。

整體思路是從狀態估計——最大後驗估計——最小二乘——非線性最小二乘

狀態估計——最大後驗估計

非線性優化在slam中的應用是對物體自身的狀態估計，狀態變數為xi=[ri , pi , vi , bi ] ，這幾個變數分別時旋轉、平移、速度和偏置。狀態估計問題就是在已知感測器輸入u，和觀測資料z時，對xi的條件概率分布進行估計。

現在只考慮觀測資料對狀態變數的影響，即計算p (xk |zk ），xk 表示所有關鍵幀的狀態，zk= ，(i,j )∈kk，ci為相機在i時刻的測量值，iij為從i到j時刻imu的測量資料。根據最大後驗估計（map）,

p (xk |zk ）=p (zk |xk )p (x0），p (x0）為先驗，即已知的物體狀態。最後化簡為

最大後驗估計——最小二乘

對於最大後驗估計，我們的目的是最大化概率p (xk |zk ）。觀測方程的表達假定為zk=h(xk)+vij，假定雜訊vij服從零均值的高斯分布，其協方差為σ，vij~n(0,σ)。

那麼對於p (zk |xk )=n（h(xk)，σ)，這仍然是乙個高斯分布。計算xk使得這個概率分布趨於最大，使用最小化負對數的方法。

考慮乙個高斯分布x~n(u,σ)，其概率密度函式的展開形式為

求負對數後變成

取完負對數的最小值，就是其概率密度函式的最大值，注意紅框內的部分，其前面1/2ln()的值是乙個固定值，因此只和紅框內的大小變化有關，現在問題就變成了乙個最小二乘問題。

定義zk,j —h(xk,yj)=ev,k ，用真實值減去觀測值，取得觀測方程的誤差，同理對運動方程也求誤差，最小化兩者的誤差

對於on-manifold preintegration for real-time visual-inertial odometry中的最小二乘問題代入後就是，這裡的r表示測量殘差。

最小二乘——非線性最小二乘

通過以上，我們將狀態估計轉化為最小二乘問題，如何求解這個最小二乘問題。引入非線性優化，非線性優化方法主要使用高斯牛頓法、列文伯格-馬誇爾特方法，具體不展開。