A. 卡爾曼濾波理解與實現
本文為離散卡爾曼濾波演算法的一 一個簡明教程，從演算法思想、實現過程、理論推導和程序實現四個方面闡述和分析了卡爾曼濾波演算法。
XU Ruilin完成本教程主要部分的編寫，WANG Xuejun完成第3節的編寫，ZHU Ximin完成2.2節的編寫，WEN Shuhan完成2.3節的編寫，MAO Bo完成全文整理、修訂和排版。
卡爾曼濾波（Kalman Filtering）及其一系列的優化和改進演算法是目前在求解運動狀態推算問題上最為普遍和高效的方法。 魯道夫·卡爾曼 （Rudolf Emil Kalman） 在NASA埃姆斯研究中心訪問時，發現他的方法適用於解決阿波羅計劃的軌跡預測問題。阿波羅飛船的導航電腦就是使用這種濾波器進行軌跡預測。
卡爾曼濾波尤其適用於動態系統，這種方法對於內存要求極低而運算速度快，且能夠保持較好的計算精度，這使得這種方法非常適合解決實時問題和應用於嵌入式系統，也就是說，卡爾曼濾波天然的適用於解決艦艇指控系統的航跡推算問題。在接下來的內容里，我們將逐步領會卡爾曼濾波的這些絕佳特點。
不過，現在我們先從復雜的艦艇航跡推算問題中解脫出來，從一個更加熟悉和簡單的問題中來理解這個濾波演算法的思想、過程和演算法。
假設有一輛無人車WALL-E，需要導引它從A點到達B點，共有兩種手段（ 圖1 ）：
顯然，兩種方法都有一定的誤差。如果單獨採用某一種方法進行定位，WALL-E在誤差的影響下將無法到達B點。因此，需要將兩種方法結合起來，得到一個更加精確的結果，這就是卡爾曼濾波要解決的問題。
卡爾曼濾波方法如何看待我們的問題呢？在探究這個問題之前，我們先對問題進行抽象，並用數學語言來描述我們的問題。
我們用矢量 來描述WALL-E的運動狀態，這個列矢量 包括位置矢量 和速度矢量 兩個分量。在WALL-E的問題上，我們現在不知道位置 和速度 的准確值，但是知道WALL-E的運動模型滿足 狀態方程 ，定位的方法，也即觀測WALL-E運動狀態的方法滿足 觀測方程 . 當然，我們也知道，這兩種方法都存在一定的誤差 ，那麼我們的問題就可以轉化為一個優化問題——
在這一優化問題中，目標函數是要使預測（估計）誤差最小，同時約束於估計方法 和 的條件下。在卡爾曼濾波中，我們的估計原則（也就是最小化估計誤差的原則）是 最小方差無偏估計 [1] ，我們將通過後面的過程分析來說明這一點。
在我們正式開始引入公式分析卡爾曼濾波問題之前，我們還必須解決一個問題------把連續的線性系統離散化，也就是將連續時域問題轉化為時間序列問題。當然，目前我們只討論線性系統的情況，關於非線性系統問題，我們有擴展卡爾曼濾波（Extended Kalman Filtering, EKF）和無跡卡爾曼濾波（Unscented Kalman Filtering, UKF）兩種方法來求解。
補充內容------連續線性時變系統的離散化
設連續線性時變系統的時域狀態方程為
若采樣周期為 ，則從時刻 到時刻 ，有
令 ， ，則離散化的狀態方程為
通過對線性系統的離散化處理，我們現在可以考慮每一個時刻WALL-E的運動狀態。接下來，我們將用 來表示在 時刻運動狀態的最優估計值；用 表示用 時刻對 時刻的狀態預測值；用 表示對 時刻綜合預測和觀測兩種方法的最優估計值。
在估計WALL-E位置的問題上，假定我們已經知道它是勻速直線運動，WALL-E身上還攜帶有一個GPS感測器可以提供它的位置信息，WALL-E在前進過程中可能會遇到一些情況，比如停止前進或是受到風的影響。
加入我們已知的是WALL-E上一個時刻的最佳估計狀態，即k-1時刻的位置和速度，要求的是下一時刻即k時刻的最佳估計狀態，即k時刻的位置和速度，我們可以發現有兩種方法可以得到它的k時刻的狀態：
一種是通過WALL-E設定程序計算得到下一秒的狀態，比如現在設定是勻速直線運動，那麼下一秒的速度應該是恆定不變的，而位置則是在上一秒位置的基礎上加上時間乘以速度即一秒內走過的路程，但是現實生活中並不是理想的，機器人會受到摩擦力、風力等的影響，當然也可能會有頑皮的小孩擋住他前進的道路，這些因素使得WALL-E在k時的真實狀態與我們計算得到的數據有所不同。
另一種是通過WALL-E所攜帶的GPS來確定它的位置，因為GPS是測量出的就是WALL-E的實時狀態，因此它比較准確。但是GPS測量k時刻的狀態有兩個問題，一是GPS只能測出WALL-E的位置，而測不出它的速度；二是GPS感測器測量的時候也會有儀器的誤差，只能說它是比較准確的，比較接近真實值的。
那麼接下來問題來了，我們如何得到k時刻WALL-E的真實狀態呢？
我們將第一種方法得到的狀態值稱為預測值，第二種方法得到的狀態值稱為測量值，對汽車的最佳估計就是將這兩部分信息結合起來，盡量的去逼近k時刻的真實值。
下面再深入一些思考，怎麼將這兩部分結合起來？
在初始時間k-1， 是WALL-E的最佳估計值，WALL-E其實可以是估計值附近的任何位置，並且這種不確定性由該概率密度函數描述。WALL-E最有可能在這個分布的平均值附近。在下一個時間，估計的不確定性增加，用一個更大的方差表示，這是因為在時間步驟k-1和k之間，WALL-E可能收到了風力的影響，或者腳可能向前滑了一點，因此，它可能已經行進了與模型預測的距離不同的距離。
WALL-E位置的另一個信息來源來自測量，方差表示誤差測量的不確定性，真正的位置同樣可以是平均值附近的任何位置。
預測值和測量值，對WALL-E的最佳估計是將這兩部分信息結合起來，將兩個概率函數相乘得到另一個高斯函數，該估計值的方差小於先前估計值，並且該概率密度函數的平均值為我們提供了WALL-E位置的最佳估計。
以下，我們將進行e的運算推導
設：
則有實際目標變數的表達式：
數學模型中目標變數的表達式：
實際模型中測量變數的表達式：
數學模型中測量變數的表達式：
將目標變數的實際值和估計值相減：
將上述方程帶入誤差e的表達式，我們可得出誤差e的解析解：
從推導結果中我們不難看出，估計值和實際值的誤差隨時間呈指數形式變化，當（F-KH）<1時，隨著時間的推移，會無限趨近於零，也就是意味著估計值和實際值相吻合。這就是為什麼卡爾曼濾波器可以完美預測出目標狀態值的原理。
在估計WALL-E位置的問題上，我們不知道位置 和速度 的准確值，但是我們可以給出一個估計區間（ 圖5.a ）。卡爾曼濾波假設所有的變數是隨機的且符合高斯分布（正態分布）。每個變數有一個均值 和一個方差 （ 圖5.b ）。而 圖5.c 則表示速度和位置是相關的。
假如我們已知上一個狀態的位置值，現在要預測下一個狀態的位置值。如果我們的速度值很高，我們移動的距離會遠一點。相反，如果速度慢，WALL-E不會走的很遠。這種關系在跟蹤系統狀態時很重要，它給了我們更多的信息：一個觀測值告訴我們另一個觀測值可能是什麼樣子。這就是卡爾曼濾波的目的------從所有不確定信息中提取有價值的信息。
根據數理統計知識，我們知道這種兩個觀測值（隨機變數）之間的關系可以通過一個協方差矩陣
描述（ 圖6 ）。
我們假設系統狀態的分布為 高斯分布（正態分布） ，所以在 時刻我們需要兩個信息：最佳預估值 及其協方差矩陣 （如式(2)所示）。
下一步，我們需要通過 時刻的狀態來預測 時刻的狀態。請注意，我們不知道狀態的准確值，但是我們的預測函數並不在乎，它僅僅是對 時刻所有可能值的范圍進行預測轉移，然後得出一個k時刻新值的范圍。在這個過程中，位置 和速度 的變化為
我們可以通過一個狀態轉移矩陣 來描述這個轉換關系
同理，我們更新協方差矩陣 為
到目前為止，我們考慮的都是勻速運動的情況，也就是系統沒有對WALL-E的運動狀態進行控制的情況。那麼，如果系統對WALL-E進行控制，例如發出一些指令啟動或者制動輪子，對這些額外的信息，我們可以通過一個向量 來描述這些信息，並將其添加到我們的預測方程里作為一個修正。假如我們通過發出的指令得到預期的加速度 ，運動狀態方程就更新為
引入矩陣表示為
式中 稱為控制矩陣， 稱為控制向量（例如加速度 ）。當然，如果沒有任何外界動力影響的系統，可以忽略這一部分。
我們增加另一個細節，假如我們的預測轉換矩陣不是100%准確呢，會發生什麼？如果狀態只會根據系統自身特性演變，那樣將不會有任何問題。如果所有外界作用力對系統的影響可以被計算得十分准確，那樣也不會有任何問題。但是如果有些外力我們無法預測，例如我們在跟蹤一個四軸飛行器，它會受到風力影響；或者在跟蹤一個輪式機器人，輪子可能會打滑，地面上的突起會使它減速。我們無法跟蹤這些因素，而這些不確定事件發生時，預測方程將會失靈。因此，我們將這些不確定性統一建模，在預測方程中增加一個不確定項。
通過這種方式，使得原始狀態中的每一個點可以都會預測轉換到一個范圍，而不是某個確定的點（ 圖7.a ）。 可以這樣描述------ 中的每個點移動到一個符合方差 的高斯分布里（ 圖7.b ）。換言之，我們把這些不確定因素描述為方差為 的高斯雜訊，並用 表示。這樣就會產生一個新的高斯分布，方差不同，但是均值相同（ 圖7.c ）。
通過對 的疊加擴展，得到完整的預測轉換方程為
新的預測轉換方程只是引入了已知的系統控制因素。新的不確定性可以通過之前的不確定性計算得到。到這里，我們得到了一個模糊的估計范圍------通過 和 描述的范圍。
我們之前的工作仍然是在使用運動模型一種方法來估計系統的狀態，現在，我們要把另一種方法，也就是觀測（本問題中為GPS定位）考慮進來，以進一步修正對運動狀態的估計（ 圖8 ）。
我們用矩陣 來描述觀測方法的作用，於是有
再加入觀測雜訊 ，觀測方程為
從控制論的角度出發，我們定義新息（也即觀測值與預測值的誤差）為
當然我們也知道，觀測本身也會存在誤差，比如本問題中的GPS定位精度僅有10m. 因此，我們用矩陣 來描述這種不確定性（ 圖10 及 圖11.a ）。
這時，我們新息的協方差為
現在我們需要把兩種方法得到的可能性融合起來（ 圖11.b ）。對於任何狀態，有兩個可能性：1. 感測器的觀測值更接近系統真實狀態；2. 模型推算的估計值更接近系統真實狀態。如果有兩個相互獨立的獲取系統狀態的方式，並且我們想知道兩者都准確的概率值，於是我們可以通過加權來解決更相信誰的問題（ 圖11.c ）。
我們現在知道，系統模型的狀態預測 與對系統的狀態觀測 服從高斯分布，把這個問題抽象一下就是——
根據我們的一個估計准則------ 最小方差估計 ，那麼這個問題可以轉化為優化問題求解
求導數（差分）得
則 ，從而
當維度高於一維時，我們用矩陣來描述，有
這里的 稱為 卡爾曼增益 （Kalman Gain），也就是我們在解決更信任哪種方法時的偏向程度。
如果我們從兩個獨立的維度估計系統狀態，那麼根據系統模型的預測為
通過感測器的觀測為
我們結合著兩種方法得到
由 可知，卡爾曼增益為
將 約去（ 中也含有 項），得
此時的卡爾曼增益實際為
我們最後再來驗證一下 估計的無偏性 ——
這里我們設 時刻的真值為 ，由於
由於 （ 從初值而來的無偏傳遞性 ）可知 ，即卡爾曼濾波滿足無偏估計准則。顯然，其中要求系統雜訊和觀測雜訊是不相關、零期望的白雜訊，且是線性系統，初始時刻的狀態估計是無偏的。當這些條件不能滿足時，卡爾曼濾波的估計結果是有偏的。
到這里，我們已經獲得了卡爾曼濾波的全部要素。我們可以把整個過程總結為3個基本假設
假設一 和 都是零均值高斯白雜訊，也即 ，
假設二 與 無關，也即
假設三 系統初值 的均值和方差已知，且 與 均不相關。
以及5個基本方程 方程一 狀態預測
方程二 協方差預測
方程三 卡爾曼增益
B. 什麼是濾波演算法
卡爾曼濾波器（Kalman Filter）是一個最優化自回歸數據處理演算法（optimal recursive data processing algorithm）。對於解決很大部分的問題，他是最優，效率最高甚至是最有用的。他的廣泛應用已經超過30年，包括機器人導航，控制，感測器數據融合甚至在軍事方面的雷達系統以及導彈追蹤等等。近年來更被應用於計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。
最佳線性濾波理論起源於40年代美國科學家Wiener和前蘇聯科學家Kолмогоров等人的研究工作，後人統稱為維納濾波理論。從理論上說，維納濾波的最大缺點是必須用到無限過去的數據，不適用於實時處理。為了克服這一缺點，60年代Kalman把狀態空間模型引入濾波理論，並導出了一套遞推估計演算法，後人稱之為卡爾曼濾波理論。卡爾曼濾波是以最小均方誤差為估計的最佳准則，來尋求一套遞推估計的演算法，其基本思想是：採用信號與雜訊的狀態空間模型，利用前一時刻地估計值和現時刻的觀測值來更新對狀態變數的估計，求出現時刻的估計值。它適合於實時處理和計算機運算。
現設線性時變系統的離散狀態防城和觀測方程為：
X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1)
Y(k) = H(k)·X(k)+N(k)
其中
X(k)和Y(k)分別是k時刻的狀態矢量和觀測矢量
F(k,k-1)為狀態轉移矩陣
U(k)為k時刻動態雜訊
T(k,k-1)為系統控制矩陣
H(k)為k時刻觀測矩陣
N(k)為k時刻觀測雜訊
則卡爾曼濾波的演算法流程為：
預估計X(k)^= F(k,k-1)·X(k-1)
計算預估計協方差矩陣
C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)'
Q(k) = U(k)×U(k)'
計算卡爾曼增益矩陣
K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1)
R(k) = N(k)×N(k)'
更新估計
X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^]
計算更新後估計協防差矩陣
C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)'
X(k+1) = X(k)~
C(k+1) = C(k)~
C. surf演算法C語言編寫，要做嵌入式開發，不要C++和基於OPENCV的
surf借鑒了sift中簡化近似的思想，將DOH中的高斯二階微分模板進行了近似簡化，使得模板對圖像的濾波只需要進行幾個簡單的加減法運算，並且，這種運算與濾波模板的尺寸有關。實驗證明surf演算法較sift演算法在運算速度上要快3倍左右。
1積分圖像
surf演算法中要用到積分圖像的概念。藉助積分圖像，圖像與高斯二階微分模板的濾波轉化為對積分圖像的加減運算。積分圖像（IntegralImage）的概念是由viola和Jones提出來的，而將類似積分圖像用於盒子濾波是由Simard等人提出。
積分圖像中任意一點（i,j）的值為ii(i,j)為原圖像左上角到任意點（i,j）相應的對角線區域灰度值的總和即：
公式中，I(x`,y`)表示原圖像中點（i`,j`）的灰度值，ii（x,y）可以由下面兩公式迭代計算得到：
公式中，S（x,y）表示一列的積分，且S（i,-1）=0,ii(-1,j)=0.求積分圖像，只需對原圖像的所有像素素進行一遍掃描。下面的代碼為c++語言的實現
pOutImage[0][0]=pInImage[0][0];
for(intx=1,x<nWidth;i++)
{
pOutImage[x][0]=pInImage[x-1][0]+pInImage[x][0];
}
for(inty=1;y<nHeight;y++)
{
intnSum=0;
for(intx=0;x<nWidth;x++)
{
nSum=pInImage[x][y];
pOutImage[x][y]=pInImage[x][y-1]+nSum;
}
}
如圖表示，在求取窗口w內的像元灰度和時，不管窗口W的大小如何，均可利用積分圖像的4個對應點（i1,j1）（i2,j2）（i3,j3）（i4,j4）的值計算的到。也就是說，求取窗口W內的像元灰度和與窗口的尺寸是無關的。窗口W內的像元的灰度和為
Sum（W）=ii(i4,j4)-ii(i2,j2)-ii(i3,j3)+ii(i1,j1)
下面看以截圖，相信都可以看懂
關於矩形區域內像素點的求和應該是一種簡單重復性運算，採用這種思路總體上提高了效率。為什麼這么說呢？假設一幅圖片共有n個像素點，則計算n個位置的積分圖總共的加法運算有n-1次（注意：可不是次哦，要充分利用遞推思想），將這些結果保存在一個跟原圖對應的矩陣M中。當需要計算圖像中某個矩形區域內的所有像素之和是直接像查表一樣，調出A,B,C,D四點的積分圖值，簡單的加減法（注意只需要三次哦）即可得到結果。反之，如果採用naive的方式直接在原圖像中的某個矩形區域內求和，你想想，總共可能的矩形組合有多少？！！且對於一幅圖像n那是相當大啊，所以2^n
那可是天文數字，而且這裡面絕大部分的矩形有重疊，重疊意味著什麼？在算求和的時候有重復性的工作，其實我們是可以有效的利用已經計算過的信息的。這就是積分圖法的內在思想：它實際上是先計算n個互不重疊（專業點說是不相交）的矩形區域內的像素點求和，充分利用這些值（已有值）計算未知值，有點類似遞推的味道...這就完全避免了重復求和運算。
這樣就可以進行2種運算：
（1）任意矩形區域內像素積分。由圖像的積分圖可方便快速地計算圖像中任意矩形內所有像素灰度積分。如下圖2.3所示，點1的積分圖像ii1的值為(其中Sum為求和):
ii1=Sum（A）
同理，點2、點3、點4的積分圖像分別為：
ii2=Sum(A)+Sum(B);ii3=Sum(A)+Sum(C);ii4=Sum(A)+Sum(B)+Sum(C)+Sum(D);
矩形區域D內的所有像素灰度積分可由矩形端點的積分圖像值得到:
Sum(D)=ii1+ii4-(ii2+ii3)(1)
(2)特徵值計算
矩形特徵的特徵值是兩個不同的矩形區域像素和之差，由(1)式可以計算任意矩形特徵的特徵值，下面以圖2.1中特徵原型A為例說明特徵值的計算。
如圖2.4所示，該特徵原型的特徵值定義為:
Sum（A）-Sum（B）
根據（1）式則有：Sum(A)=ii4+ii1-(ii2+ii3);Sum(B)=ii6+ii3-(ii4+ii5);
所以此類特徵原型的特徵值為:
(ii4-ii3)-(ii2-ii1)+(ii4-ii3)-(ii6-ii5)
另示：運用積分圖可以快速計算給定的矩形之所有象素值之和Sum(r)。假設r=(x,y,w,h),那麼此矩形內部所有元素之和等價於下面積分圖中下面這個式子:
Sum(r)=ii(x+w,y+h)+ii(x-1,y-1)-ii(x+w,y-1)-ii(x-1,y+h)
由此可見，矩形特徵特徵值計算只與此特徵端點的積分圖有關，而與圖像坐標值無關。對於同一類型的矩形特徵，不管特徵的尺度和位置如何，特徵值的計算所耗費的時間都是常量，而且都只是簡單的加減運算。其它類型的特徵值計算方法類似。
D. 圖像處理之雙邊濾波演算法
雙邊濾波是一種非線性的濾波方法，是結合圖像的空間鄰近度和像素值相似度的一種折中處理，同時考慮空域信息和灰度相似性，達到保邊去噪的目的，具有簡單、非迭代、局部處理的特點。之所以能夠達到保邊去噪的濾波效果是因為濾波器由兩個函數構成：
一個函數是像素歐式距離決定濾波器模板的系數，另一個是由像素的灰度差值決定濾波器模板的系數。
其綜合了高斯濾波器（Gaussian Filter）和α-截尾均值濾波器（Alpha-Trimmed mean Filter）的特點。高斯濾波器只考慮像素間的歐式距離，其使用的模板系數隨著和窗口中心的距離增大而減小；Alpha截尾均值濾波器則只考慮了像素灰度值之間的差值，去掉α%的最小值和最大值後再計算均值。
雙邊濾波器使用二維高斯函數生成距離模板，使用一維高斯函數生成值域模板。
雙邊濾波器中，輸出像素的值依賴於鄰域像素的值的加權組合，其公式如下：
其中(k,l)為模板窗口的中心坐標；(i,j)為模板窗口的其他系數的坐標；σd為高斯函數的標准差。 使用該公式生成的濾波器模板和高斯濾波器使用的模板是沒有區別的。
值域模板系數的生成公式如下：
其中，函數f(x,y)表示要處理的圖像，f(x,y)表示圖像在點(x,y)處的像素值；(k,l)為模板窗口的中心坐標；(i,j)為模板窗口的其他系數的坐標；σr為高斯函數的標准差。
將上述兩個模板相乘就得到了雙邊濾波器的模板,其公式如下：