感測器濾波演算法

1. 什麼是濾波演算法

卡爾曼濾波器（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)~

2. LMS自適應濾波演算法中要求的期望輸出和濾波器的輸入之間有什麼區別，採集的實際信號往往是無法知道期望輸出

自適應濾波演算法有幾種應用類型，不同類型的目的、原理和手段不同，所以相對應的選取輸入和期望信號也很不一樣。
1.系統辨識：當我們想描述一個未知系統（如一組復雜的模擬電路），解析的算出系統的沖擊響應或者系統函數是比較困難的。這時，我們就可以用未知系統的輸入和輸出訓練自適應濾波器（未知系統的輸入作為自適應濾波器的輸入，未知系統的輸出作為自適應濾波器的期望信號，當自適應濾波器收斂後，對應的濾波器就可以看做是未知系統的近似）。

3. 卡爾曼濾波理解與實現

本文為離散卡爾曼濾波演算法的一 一個簡明教程，從演算法思想、實現過程、理論推導和程序實現四個方面闡述和分析了卡爾曼濾波演算法。

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個基本方程 方程一 狀態預測

方程二 協方差預測

方程三 卡爾曼增益

4. 卡爾曼濾波

將預測值和測量值進行結合，對系統狀態進行最優估計的演算法。

在連續變化的系統中使用卡爾曼濾波是非常理想的，它具有佔用內存小的優點（除了前一個狀態量外，不需要保留其它歷史數據），並且速度很快，很適合應用於實時問題和嵌入式系統。

根據k-1時刻的系統狀態預測k時刻系統狀態。

考慮外部因素控制的影響
外部因素會對系統進行控制，從而帶來一些與系統自身狀態沒有相關性的改變。其中 成為控制矩陣， 稱為控制向量，如果沒有外部控制，這部分可以忽略。

外部雜訊因素
在每次預測之後，我們可以添加一些新的不確定性來建立這種與「外界」（即我們沒有跟蹤的干擾）之間的不確定性模型

小結：
由上兩式可知，新的最優估計是根據上一最優估計預測得到的，並加上已知外部控制量的修正。 而新的不確定性由上一不確定性預測得到，並加上外部環境的干擾。

加入感測器觀測數據
卡爾曼濾波的一大優點就是能處理感測器雜訊，我們的感測器或多或少都有點不可靠，並且原始估計中的每個狀態可以和一定范圍內的感測器讀數對應起來。 從測量到的感測器數據中，我們大致能猜到系統當前處於什麼狀態。但是由於存在不確定性，某些狀態可能比我們得到的讀數更接近真實狀態。
感測器早上用協方差 表示，該分布的均值 是我們讀取到的感測器數據。
於是我們得到兩個高斯分布，一個是預測值附近，一個是感測器讀數附近。把兩個具有不同均值和方差的高斯分布相乘，得到一個新的具有獨立均值和方差的高斯分布。
結果如下，其中，K為卡爾曼增益。

總結：
我們可以用這些公式對任何線性系統建立精確的模型，對於非線性系統來說，我們使用擴展卡爾曼濾波，區別在於EKF多了一個把預測和測量部分進行線性化的過程。

參考文章：
http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
https://blog.csdn.net/u010720661/article/details/63253509

5. 如何通俗並盡可能詳細解釋卡爾曼濾波

卡爾曼濾波（Kalman filtering）一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的演算法。由於觀測數據中包括系統中的雜訊和干擾的影響，所以最優估計也可看作是濾波過程。

斯坦利·施密特(Stanley Schmidt)首次實現了卡爾曼濾波器。卡爾曼在NASA埃姆斯研究中心訪問時，發現他的方法對於解決阿波羅計劃的軌道預測很有用，後來阿波羅飛船的導航電腦使用了這種濾波器。 關於這種濾波器的論文由Swerling (1958), Kalman (1960)與 Kalman and Bucy (1961)發表。

數據濾波是去除雜訊還原真實數據的一種數據處理技術, Kalman濾波在測量方差已知的情況下能夠從一系列存在測量雜訊的數據中，估計動態系統的狀態. 由於, 它便於計算機編程實現, 並能夠對現場採集的數據進行實時的更新和處理, Kalman濾波是目前應用最為廣泛的濾波方法, 在通信, 導航, 制導與控制等多領域得到了較好的應用。

6. Apollo卡爾曼濾波與EKF

感測器存在誤差——需要跟蹤——卡爾曼濾波

障礙物跟蹤、車道線跟蹤、障礙物預測以及定位等領域；

線性系統，且參數是符合高斯分布 ，即完全可以被均值和協方差參數化:X∼N(μ, σ²)。

預測狀態（prediction）與觀測狀態(measurement)分別加權求和作為新時刻的狀態向量。

prediction ：

(狀態協方差矩陣，表示系統不確定度)

measurement update :

（實際觀測到的測量值 z 與預測值 x' 之間差值 y）

（當前狀態向量 x 的更新）

（根據卡爾曼增益，更新了系統的不確定度 P）

其中，
F （state transition matrix）：狀態轉移矩陣(運動學推導)；

B ：控制輸入矩陣；

u ：引起運動的輸入向量（如油門、剎車、轉角）；

w ：過程向量中的雜訊因素，符合高斯分布，w∼N(0,Q)；

P （state covariance matrix）：在卡爾曼濾波器初始化時會很大，隨著越來越多的數據注入濾波器中，不確定程度會變小；

Q （process covariance matrix）：過程雜訊，即無法用 x'=Fx+u 表示的雜訊，比如車輛運動時突然到了上坡，這個影響是無法用之前的狀態轉移估計的；（由於 Q 對整個系統存在影響，但又不能太確定對系統的影響有多大。工 程上，我們一般將 Q 設置為單位矩陣參與運算 ）

H （Measurement Matrix）：測量矩陣， 由於感測器的測量值不一定就是直接的狀態向量 ，如毫米波雷達是角度和位置信息，這里H就是轉換矩陣，將預測向量轉為測量數據；

R （measurement covariance matrix）：測量雜訊矩陣，表示的是測量值與真值之間的差值。一般情況下，感測器的廠家會提供該值；

S 只是為了簡化公式，寫的一個臨時變數；

K （Kalman Gain）：卡爾曼增益，也即y的權重。

問題 ：針對線性系統，且符合高斯分布。

當狀態向量與測量量之間映射關系是 非線性 時，這將使得卡爾曼濾波的過程和測量符合高斯分布的假設無效。

如毫米波雷達，返回數據是基於極坐標系，包含：

此時，H轉移矩陣（狀態向量向測量數據轉換），也即笛卡爾坐標向極坐標映射：

這個映射就是非線性函數。

EKF 使用 局部線性逼近非線性模型 ，通過計算當前狀態估計的 一階泰勒展開 得出。一階的逼近也叫 雅克比矩陣 。

如對上 泰勒展開（在均值u處，本例為0），
求出雅克比矩陣如下：

對比公式

2. 自動駕駛中的感測器融合演算法：第一部分 - 卡爾曼濾波器和擴展卡爾曼濾波器

5. 詳解卡爾曼濾波原理

7. 為什麼要進行遙控和感測器的數據濾波演算法處理

基本條件：具備數據採集、FFT、低通、高通、帶通和帶阻數字濾波器等；
若作低通濾波 輸入原始數據、作FFT、選低通濾波、設置截止頻率、輸出結果；
其它類似。如果有數據處理，這類工作是很簡單的。

8. 對壓力感測器得到的信號應進行怎樣的濾波，高通還是低通

這個應該根據你的壓力感測器和你的應用場合來選擇如何去除雜訊，可能單單的低通無法滿足，比如如果你用的是manometer，可能需要較復雜的演算法得到比較穩定的壓力輸出信號

9. AD數字信號（稱重感測器）的防抖動濾波用哪種演算法穩定

抖動一般是在0兩側分布的，所以幾次采樣求和取平均應該就可以了，而且還快，本身這種就是穩健估計，不易發散的

10. 感測器的信號抖動怎麼去除

請問這是需要加一個低通濾波嗎？我試了用RC濾波器可是看不出效果。是不是我的R和C的參數選擇不合適？是否有其他更簡便的方法呢？只要能把那個抖動去掉就可以。

另外，這種隨機變化的感測器信號，它的頻率怎麼確定啊？感測器的輸出大概40毫秒變化一次，也就是說可以認為是每一秒采樣25次，是不是就是25Hz的信號呢？