數據融合之卡爾曼濾波示例
哎，難受，知乎把我的完美格式打亂成這樣子。
卡爾曼幾乎是做數據融合都知道的一個演算法了把。卡爾曼濾波能夠實現從有雜訊的感測器數據中獲取我們的較為準確的信息。而且通過卡爾曼，我們甚至能夠去推算一些不可直接測量的值。
1.高斯知識的簡單回顧
在卡爾曼濾波器中，我們的任務就是估計出物體最佳的期望以及方差出來。接下來我們來些一個簡單的高斯函數
def f(mu,sigma,x):
return 1/sqrt(2.*pi*sigma) * exp(-.5*(x - mu)**2/sig,a2)
均值代表你的最佳估計，方差代表你的不確定性
對於多元的高斯分布，如果高斯分布是斜的，表示兩個維度的不確定性是相關的
高斯另外一個條件概率的性質：
這個可以看作是給定的信息下，能夠推測出的概率是多少。
以上是一個兩個高斯信號的條件分布，對於條件分布，如果對於變數B，B我們可以看成是感測器提供的信息，我們非常非常不確定的話，那麼自然很大很大，這樣的逆就很小很小，最後其實對均值和方差那一項就沒有影響了，也就是說B沒有提供任何信息量。
這個公式反應了兩個高斯信號相乘的話，會得到一個方差更小的高斯分布，直觀上的理解就是我們的信息更多了,那麼我們的不確定性也就減少了，這實際上也是卡爾曼濾波的更新過程的簡單版
現在讓我們寫一個更新的函數
def update(mean1,var1,mean2,var2):
new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2)
new_var = 1 / (1 / var1 + 1 / var2)
return [new_mean new_var]
2. 一個移動機器人定位的例子
這個例子大家都應該見過，也就是一個不斷測量和預測的過程實現定位的過程。
至於為什麼運動會影起方差的減少，可以看下下面的圖的過程。
以上是我們的運動更新，可以看到 由於運動的不確定性，它會導致運動之後的不確定性增加，也就是方差變大
def predict(mean1,var1,mean2,var2):
new_mean = mean1 + mean2
new_var = var1 + var2
return [new_mean,new_var]
現在我們我們來實現一個一維的卡爾曼濾波器，運用我們剛才的 預測函數(運動模型) 以及 更新函數（測量模型），假定初始的位置為0，方差10000，
measurements = [5., 6., 7., 9., 10.]
motion = [1., 1., 2., 1., 1.]
measurement_sig = 4.
motion_sig = 2.
mu = 0. #init pos
sig = 10000. #init variance
for i in range(len(measurements)):
[mu, sig] = update(mu, sig, measurements[i], measurement_sig)
print(update:,[mu, sig])
[mu, sig] = predict(mu, sig, motion[i], motion_sig)
print(predict:,[mu, sig])
如果初始值是5的話，那麼結果會是非常好，但是這裡我們將初始值設置為0，且不確定性特別高。
運行程序 你會發現你的第一次位置估計結果差不多是5，原因在於你的初始不確定性太高了，根據我們剛才的公式就可以測量出來了。 第一次測量主導了結果 ，而且你的不確定性也降了很多，可以根據update那個數學公式推測出原因。
update: [4.999000199960007, 1.9996000799840032]
predict: [5.999000199960007, 3.9996000799840035]update: [5.999500074988752, 1.9999000149977504]predict: [6.999500074988752, 3.9999000149977504]update: [6.999750034370274, 1.9999750034370276]predict: [8.999750034370274, 3.9999750034370276]update: [8.999875016794618, 1.9999937508397312]predict: [9.999875016794618, 3.999993750839731]update: [9.999937508348493, 1.9999984377087126]predict: [10.999937508348493, 3.9999984377087126]
簡單描述下上面這個例子
假設一開始處於0位置，但是不確定性非常高。
接下來進行第一次測量，可以看到變為4.999，方差下降為1.996.
接下來motion 了 1 步，機器人的位置變為5.999，方差增加為3.9996.
這個過程不斷循環下去。
目前為止，我們已經實現了一個1D的卡爾曼濾波，總結起來就是
測量會減少系統的不確定性
預測會增加系統的不確定性（也就是運動的時候）
現在考慮2D的情況，想像我們有一個雷達，能夠測出物體的位置，卡爾曼可以允許我們預測物體的速度，然後根據速度對未來做出更好的預測。 注意感測器本身只能看到位置，但是看不到速度，速度是從看到的多個位置推斷出來的，在跟蹤的應用當中，即使它沒有直接測量，也能夠得出。
This set of equation has been able to propagate constrains from subsequent measurements back to this unobservable variable , velocity ,so we can observe the vehicle also . This is really key to understand Kalmen filter .
But because those 2 thing interact , subsequent observations of the observable variable give us information about these hidden variables ,so we can also estimate what these the hidden variables are .
只要這些量是相關的，其實我們就可以推斷出來啦。畢竟建立了微分方程之後，肯定就可以了呀
這個可以看下Sebatian 的視頻
3.運動模型
4.測量模型
5.測量模型
測量模型根據你感測器的不同也有很多變化，比如激光返回位移+角度，攝像頭返回像素點，具體的模型其實都相當成熟了。
6.貝葉斯濾波框架
剛才講的卡爾曼例子是屬於貝葉斯框架裡面的。
貝葉斯濾波器是遞歸狀態濾波器的一個框架，是一大類方法的統稱，但是它是有不同的實現方式
可以根據以下屬性進行分類：
- 運動模型與觀測模型 是線性還是非線性
- 雜訊是否是高斯分布
- 參數濾波器還是非參數濾波器
參數濾波器有卡爾曼，拓展卡爾曼以及粒子濾波
非參數濾波的話有粒子濾波器
當然現在用的很廣泛的一種優化演算法，圖優化，就不屬於濾波器的理論了。我們可以發現濾波器的這種方法是沒辦法糾正前面的狀態的，因為它是一個不斷隨著時間不斷推進的一個狀態估計。但是圖優化的話它能夠不斷調整整體的誤差，能夠實現全局優化的效果。
7.卡爾曼濾波
接下來我們先來講解一個
卡爾曼濾波器是線性高斯情況下的最優估計器了。
重點來看下卡爾曼增益的理解
對於卡爾曼增益K ，實際上是對預測值以及測量值 這兩者的權衡。我們通過兩個極端的情況來看下這個參數的作用：
- 假設我們有一個完美的感測器
- 假設我們有一個很垃圾的感測器，不能提供任何信息，這種情況相當於沒感測器
假設我們有一個完美的感測器，那麼我的 也就是我感測器的方差為0，說明測量的東西非常準確，沒有一點兒偏差。上式就變為
實際上做的就是將系統狀態的均值投影到測量空間中
實際上是把系統的方差投影到測量空間中
公式的簡單推導
預測模型
觀測模型
8.卡爾曼濾波的一個例子
接著來我們以行人慣導為例，介紹以下行人慣導中的卡爾曼濾波
行人慣導裡面，我們只使用IMU，沒有外加其他感測器，這樣做的原因是想要減少行人定位的成本，行人慣導裡面主要也是研究如何低成本地進行定位 。 這種把慣導固定再鞋子有一個優點，就是我們能夠檢測出零速時刻， 可以看下下圖直觀感受下，當左腳站直，右腳往前邁的時，實際上就是左腳零速的時候，通過這種方式來檢測出的零速可以作為我們的觀測量 。
P：為什麼之前做行人慣導要進行零速檢測呢？
實際上就是我們的觀測量就是零速。然後與我們估算的速度做比較，算出誤差。
P：為什麼要加雷達能提高精度？
實際上觀測量就是距離了，與我們估算的位移做比較，算出誤差。
貼一個大概的代碼，來自openshoe
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Zero-velocity update %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Check if a zero velocity update should be done. If so, do the
% following
%%對於預測的部分我們不需要看了，因為實際上我們的慣導就是我們每一步的預測了。
if zupt(k)==true;
% Calculate the Kalman filter gain
K=(P*H)/(H*P*H+R);
% Calculate the prediction error. Since the detector hypothesis
% is that the platform has zero velocity, the prediction error is
% equal to zero minu the estimated velocity.
z=-x_h(4:6,k);
% Estimation of the perturbations in the estimated navigation
% states
dx=K*z;
% Correct the navigation state using the estimated perturbations.
% (Subfunction located further down in the file.)
[x_h(:,k) quat]=comp_internal_states(x_h(:,k),dx,quat); % Subfunction located further down in the file.
% Update the filter state covariance matrix P.
P=(Id-K*H)*P;
% Make sure the filter state covariance matrix is symmetric.
P=(P+P)/2;
% Store the diagonal of the state covariance matrix P.
cov(:,k)=diag(P);
end
9.卡爾曼濾波的使用
P：實際用卡爾曼濾波的時候是什麼情況呢？
我們要確定好狀態變數是什麼？確定好我們的觀測方程是什麼？然後就可以套公式了。
P：另外一個值得注意的問題是：
確定過程雜訊以及測量雜訊是什麼？這個會在一定在影響我們的估計值？
確定過程雜訊是一個比較重要的過程，可以看下我的UKF最後的講解。
推薦閱讀：
1.機器人學中的狀態估計
2.概率機器人
3.Udacity上面有AI for robotics
If youd like to watch another set of videos on Kalman filters, you might check out:
The Kalman Filter from iLectureOnline
給剛學習卡爾曼的建議：
第一步：先去Udacity看下視頻先
第二步：去bilibili看下另外的卡爾曼例子
第三步：推導以下理論公式
第四步：找個數據集在Matlab上做下簡單的模擬
在這個過程中，反覆琢磨卡爾曼裡面的協方差，卡爾曼的線性高斯是什麼意思，卡爾曼增益代表什麼。
恭喜你已經掌握卡爾曼濾波。
推薦閱讀：