（二）線性系統和kf
（三）非線性系統和ekf
（四）濾波器後端小結
前端視覺里程計只能通過鄰近的幾張估計短暫時間內的運動軌跡和路標，這種方式為「漸進的」(incremental)。
後端考慮乙個更長時間內(或所有時間內)的狀態估計問題,而且不僅使用過去的資訊更新自己的狀態,也會用未來的資訊（即全域性資訊）來更新自己,這種處理方式為「批量的」(batch)。
由於方程中的觀測量都受受雜訊影響,所以要把這裡的位姿 x 和路標 y 看成服從某種概率分布（一般為高斯分布）的隨機變數,而不是單獨的乙個數。一般由於x和y的這一特點在程式中只需要儲存它們的均值（變數估計值）和協方差矩陣（度量這一估計的不確定性）。
圖 1 不確定性的直觀描述。左側:只有運動方程時,由於下乙個時刻的位姿是在上乙個時刻基礎上新增了雜訊,所以不確定性越來越大。右側:存在路標點(紅色)時,不確定性會明顯減小。因此後端要解決的問題就是：通過包含雜訊的運動資料和觀測資料,去估計狀態量的高斯分布。在程式運
行期間,我們只要維護乙個狀態量,對它進行不斷地迭代和更新即可。進一步,如果假設
狀態量服從高斯分布,那我們只需考慮維護狀態量的均值和協方差即可。
上式為簡化版運動與觀測方程，其中的x中包含了位姿和路標點兩種未知量。則第k時刻的狀態分布為：
按照bayes法則將k時刻狀態分布估計分解為似然與先驗兩部分：
上式中似然可以由觀測方程給定,而先驗部分表示當前狀態是基於過去所有的狀態估計得來的。我們首先考慮xk關於他的前乙個狀態xk-1的先驗（按照xk-1時刻為條件概率展開）：
而先驗部分的後續處理分為兩種路線：
以擴充套件卡爾曼濾波(ekf)為代表的濾波器方法：假設馬爾可夫性,簡單的一階馬氏性認為,k 時刻狀態只與 k − 1 時刻狀態有關,而與再之前的無關。
非線性優化為主體的優化框架：考慮 k 時刻狀態與之前所有狀態的關係。
考慮馬爾可夫假設時：先驗的第一部分可以簡化為：
由於 k 時刻狀態與 k − 1 之前的無關,所以就簡化成只與 x k−1 和 u k 有關的形式,與 k 時刻的運動方程對應
考慮到 k 時刻的輸入量 u k 與 k − 1 時刻的狀態無關,所以我們把 u k 拿掉。可以看到,這一項實際是 k − 1 時刻的狀態分布。在程式執行期間,我們只要維護乙個狀態量,對它進行不斷地迭代和更新即可。進一步,如果假設狀態量服從高斯分布,那我們只需考慮維護狀態量的均值和協方差即可。
運動和觀測方程用線性高斯系統簡化表示：
假設所有的狀態和雜訊均滿足高斯分布。記這裡的雜訊服從零均值高斯分布:
卡爾曼濾波器的第一步,通過運動方程確定 x k 的先驗分布。既下式的後邊部分
根據高斯分布的性質,顯然有:
其中x̂ k−1 和它的協方差 p̂ k−1第k-1時刻狀態的後驗估計（後驗估計是指最後算得的準確分布）卡爾曼濾波器的第二步，通過先驗與似然估計x k的高斯分布，既計算xk～n ( x̂ k , p̂ k )中的x̂ k , p̂ k 值：通過高斯分布指數項部分的係數對比獲得x̂ k , p̂ k 。
上面的兩個步驟可以歸納為「**」(predict)和「更新」(update)兩個步驟:
對與線性高斯系統，由於期間的高斯運算都是線性運算，故卡爾曼濾波器構成了線性系統的最優無偏估計。slam 中的運動方程和觀測方程通常是非線性函式,尤其是視覺 slam 中的相機模型,需要使用相機內參模型以及李代數表示的位姿,更不可能是乙個線性系統。由於高斯函式只有線性變換的性質，故需要先將非線性系統泰勒展開然後保留一階或二階來實現線性近似，再按照線性系統的高斯分布來計算，最後得到的就是擴充套件卡爾曼濾波器(extended kalman filter, ekf)。
令 k −1 時刻的均值與協方差矩陣為 x̂ k−1 , p̂ k−1 。在 k 時刻,我們把運動方程和觀測方程,在 x̂ k−1 , p̂ k−1處進行線性化(相當於一階泰勒展開),有:
按照線性系統的卡爾曼濾波器的步驟，先「**」：
得到先驗的均值和協方差：
之後結合觀測方程和bayes 展開式,可以推導出 x k 的後驗概率形式：
定義乙個卡爾曼增益 k k :
在卡爾曼增益的基礎上,後驗概率的形式為:
與線性高斯系統不同，在 slam 這種非線性的情況下,ekf給出了單次線性近似下最大後驗估計(map)，而不是無偏估計。濾波器方法在早期slam中佔主導地位，如今優化方法佔主導且占有明顯優勢;但是在計算資源受限，或待估計量比較簡單的場合，ekf也經常用。
ekf 的侷限：
濾波器方法在一定程度上假設了馬爾可夫性,有點像是在視覺里程計中,只考慮相鄰兩幀關係一樣。如果當前幀確實與很久之前的資料有關(例如回環),那麼濾波器就會難以處理這種情況。
ekf 濾波器僅在 x̂ k−1 處做了一次線性化,然後就直接根據這次線性化結果,把後驗概率給算了出來。而實際上,當我們離開近似點較遠的時候,一階泰勒展開並不一定能夠近似整個函式,這取決於運動模型和觀測模型的非線
性情況。如果它們有強烈的非線性,那線性近似就只在很小範圍內成立,不能認為在很遠的地方仍能用線性來近似。這就是 ekf 的非線性誤差,是它的主要問題所在。
從程式實現上來說,ekf 需要儲存狀態量的均值和方差,並對它們進行維護和更新。儲存量與狀態量呈平方增長(因為要儲存協方差矩陣n×n的矩陣)。因此,ekf slam 普遍被認為不適用於大型場景。
SLAM流程之優化後端
slam的優化後端完成工作主要是對視覺前端得到的不夠準確的相機位姿和重建地圖進行優化微調。在視覺前端中，不管是進行位姿估計還是建圖，都是利用相鄰幀之間的關係來完成的，這種依賴區域性約束且不停地鏈式進行的演算法，必將導致優化誤差逐幀累積，最終產生乙個較大的誤差漂移。因此，後端優化的思路就是從全域性 整...
SLAM十四講 後端（1）
視覺slam主要分為視覺前端和優化後端。前端也稱為視覺里程計 vo 根據相鄰影象的資訊估計處粗略的相機運動，給夠後端提供較好的初始值。vo的實現方法，按是否需要提取特徵，可分為特徵點的前端和不提特徵點的直接法前端。vo的主要問題是如何根據影象估計相機的運動 從影象選取有代表性的點 路標 在相機發生微...
SLAM中的後端優化
本節介紹slam中的後端優化過程 同時對三維點位置和相機引數進行非線性優化 原理 是一種 信賴域 的方法，當收斂速度較快時，增大信賴域使演算法趨向於高斯牛頓法 當收斂速度較慢時，減小信賴域使演算法趨向於最速下降法。優勢 速度快 可以在距離初始值較遠處得到最優解。slam優化演算法對比 演算法缺點 優...