卡爾曼濾波器又稱為最佳線性濾波器(輸出值為輸入值的線性組合),為實現簡單、純時間域的濾波器,在實現過程中,所有關於不確定性的關係(雜訊),都會用到共變異數矩陣。

卡爾曼濾波器的五個基本公式:
X(k|k-1) = F X(k-1|k-1) + B U(k) ...(1)
P(k|k-1) = F P(k-1|k-1) FT + Q ...(2)
X(k|k) = X(k|k-1) + Kg(k) (Z(k) - H X(k|k-1)) ...(3)
Kg(k) = P(k|k-1) HT / (H P(k|k-1) HT + R) ...(4)
P(k|k) = (I - Kg(k) H) P(k|k-1) ...(5)

簡單翻成白話分別為:
利用 k-1 的狀態,加上變化量,預估出 k 的狀態。 ...(1)
利用 k-1 狀態的雜訊,配合上述預估 k 狀態的雜訊,預估出 k 狀態的雜訊。 ...(2)
利用計算出 k 的狀態 與 k 的測量狀態搭配權重值(Kalman Gain) 完成更新 k 的狀態。 ...(3)
權重值(Kalman Gain) 是由步驟(2)中的 k 狀態的雜訊 與 測量雜訊 所求得。 ...(4)
最後利用 權重值(Kalman Gain) 更新 k 狀態的雜訊。 ...(5)

搭配圖示如下:
KalmanFilter




理解卡爾曼濾波器

卡爾曼濾波器的主要步驟有兩個:預估 以及 測量更新,以下舉一個例子作簡單說明。

假設要利用卡爾曼濾波器來計算房間內每分鐘的溫度變化:
  • 預估:以經驗判斷,房間內的溫度是恆溫的,也就是下一分鐘的溫度會等於目前這分鐘的溫度。

    但事實上,房間未必是恆溫的,而且以經驗判斷出的溫度也可能包含誤差,這些誤差就是所謂的高斯白雜訊,也就是說,這些誤差與前後時間無關,並且符合高斯分配。

  • 測量更新:在房間中,也利用溫度計來作溫度測量,而溫度計的測量值同樣也和實際值存在著誤差,這些誤差也是高斯白雜訊。

於是在某一分鐘內,會有兩個關於該房間溫度的值:
  1. 根據經驗的預估值 (預估值)
  2. 溫度計的值 (測量值)

接著利用這兩個值結合各自的雜訊來估算出房間的實際溫度值。

計算過程:

假設 經驗法則預估的誤差 與 溫度計的誤差 為已知的系統參數。

Step 1, state and covariance prediction
假設要預估 K 時刻的溫度,首先根據 K-1 時刻所推算的溫度來作預估。
假設 K-1 時刻所推算的房間溫度為 23度,則預估 K 時刻的房間溫度也是 23度。

又假設在 K-1 時刻所推算的房間溫度其(變異數)誤差值為 3,經驗法則預估值的(變異數)誤差為 4,則由 K-1 時刻的房間溫度及經驗法則來預估 K 時刻的溫度,其變異數為將他們各自的(變異數)誤差相加,也就是 3 + 4 = 7。

接著假設溫度計在 K 時刻的測量值為 25度,同時該溫度計的(變異數)誤差為 4度。


Step 2, Kalman gain correction and state correction
目前為止,在 K 時刻的房間溫度,預估值為 23度,測量值為 25度,那最後的溫度該如何算出?這兩個值是否有哪一個值是比較值得相信的?

這時候就要先計算 Kalman gain (Kg),Kalman gain 的值是由預估值的(變異數)誤差值與測量值的(變異數)誤差值所計算而得,其(變異數)誤差值分別為 7 與 4:
Kg = 7 / (7 + 4) = 7/11 = ~0.64

由 Kalman gain 的值,推算出 K 時刻的溫度為:
預估值 + Kalman gain * (測量值 - 預估值)
=> 23 + 0.64 * (25 - 23) = 24.28

這邊可以看出,因為預估值(23度)的(變異數)誤差為 7,而測量值(25度)的(變異數)誤差為 4,所以測量值比較值得相信,故推算出來的值會比較偏向測量值。


Step 3, covariance correction
當然所推算出來的值與實際的溫度值還是存在著(變異數)誤差,該(變異數)誤差值與預估值的(變異數)誤差值(7度)有關,計算方式為:
(1 - Kg) * 7 = 0.64 * 7 = 4.48
算出來的 4.48 度的(變異數)誤差值,對應到 K-1 時刻所推算的房間溫度誤差值 3 度。
也就是說在 K 時間,所推算出的房間溫度為 24.48 度,(變異數)誤差值為 4.48 度,接著可以再進行 K+1 時刻的房間溫度推算。

根據以上規則,卡爾曼濾波器可以一直迭代下去。

以下開始討論真正工程系統上所使用的卡爾曼濾波器。




卡爾曼濾波器演算法

首先,先介紹一個離散控制過程的系統,該系統可以用一個線性隨機微分方程式(Linear Stochastic equation)來表示:

X(k) = F X(k-1) + B U(k) + W(k)

該系統的測量值為:

Z(k) = H X(k) + V(k)

其中:
  • X(k) 是 k 時刻的系統狀態
  • U(k) 是 k 時刻對系統的控制量
  • F 和 B 是系統參數,對於多模型系統,他們是矩陣
  • Z(k) 是 k 時刻的測量值
  • H 是測量系統的參數,對於多測量系統,H 是矩陣
  • W(k) 與 V(k) 分別表示過程和測量的雜訊,被假設為高斯白雜訊,covariance 分是 Q 及 R,這裡假設他們不會隨系統狀態變化而變化

對於滿足上述條件(線性隨機微分系統,過程和測量的雜訊皆為高斯白雜訊)的系統來說,卡爾曼濾波器是最佳的資訊處理器,接著會結合他們的 covariance 來推算系統的最佳輸出值(類似上一節的溫度例子)。

首先,利用系統的過程模型,來預估下一狀態的系統,假設目前的系統狀態是 k,則根據系統的過程模型,可以基於系統的上一狀態來預估出目前的狀態:
X(k|k-1) = F X(k-1|k-1) + B U(k) ...(1)

式(1)中,X(k|k-1)是利用上一狀態預測的結果,X(k-1|k-1)是系統在上一狀態所推算的結果,U(k)是系統在目前狀態的控制量,如果沒有控制量,可以為 0。

目前為止,系統結果已經更新了,接著需要更新的是 X(k|k-1) 的 covariance,以 P 表示 covariance:
P(k|k-1) = F P(k-1|k-1) FT + Q ...(2)

至於為何需要乘上兩邊,這是因為共變異數矩陣的性質:
cov(x, x) = cov(x, x)
cov(Ax, Bx) = A cov(x, x) BT
=> cov(Ax, Ax) = A cov(x, x) AT


式(2)中,P(k|k-1) 是 X(k|k-1) 對應的 covariance,P(k-1|k-1) 是 X(k-1|k-1) 對應的 covariance,FT 表示 F 的轉置矩陣,Q 是系統過程的 covariance;式(1) 及 式(2) 就是卡爾曼濾波器5個公式當中的前兩個,也就是對系統狀態的預估。

現在有了狀態的預估結果,需要再取得目前狀態的測量值(Z(k)),接著結合預估值與測量值,就可以推算出目前狀態(k) 的最佳推算值 X(k|k):
H X(k|k) = H X(k|k-1) + Kg'(k) (Z(k) - H X(k|k-1)) ...(A)
此時對應的 covariance 為:
H P(k|k) HT = H P(k|k-1) HT - Kg'(k) H P(k|k-1) HT ...(B)

從之前的定義,可以知道 Z(k) 與 X(k) 之間的單位轉換矩陣為 H,而 Z(k) 的 covariance 為 R:
Z(k) = H X(k) + V(k)

此時 Kg'(k) 為權重比,是由 H X(k|k-1) 及 Z(k) 的 covariance 來決定,計算方式如下:
Kg'(k) = H P(k|k-1) HT / (H P(k|k-1) HT + R) ...(C)
H X(k|k-1) 的 covariance 為:H P(k|k-1) HT

接著簡化 公式 A,同時乘上 H-1,進而推導出卡爾曼濾波器的第三個基本公式:
X(k|k) = X(k|k-1) + Kg(k) (Z(k) - H X(k|k-1)) ...(3)
其中 Kg 為 Kalman Gain,為 H-1 * Kg',由公式 C 可得:
Kg(k) = P(k|k-1) HT / (H P(k|k-1) HT + R) ...(4)

目前為止,已經計算出 k 狀態下的最佳推算值 X(k|k),但是為了要讓卡爾曼濾波器可以不斷的運行下去,還需要更新 X(k|k) 的 covariance,簡化 公式 B 可以得到:
P(k|k) = P(k|k-1) - Kg(k) H P(k|k-1) =>
P(k|k) = (I - Kg(k) H) P(k|k-1) ...(5)

其中 I 為 1 的矩陣,對於單模型、單測量系統,I = 1;當系統進入 k+1 狀態時,P(k|k) 就會是 式(2) 的 P(k-1|k-1),如此,演算法就可以一直遞迴下去。

卡爾曼濾波器的基本原理如上所述,式(1) ~ 式(5) 就是卡爾曼濾波器的五個基本公式,根據這五個公式,就可以在電腦上實作卡爾曼濾波器。

以下,舉實際運行的例子。




結合溫度計實例

卡爾曼濾波器的五個基本公式:
X(k|k-1) = F X(k-1|k-1) + B U(k) ...(1)
P(k|k-1) = F P(k-1|k-1) FT + Q ...(2)
X(k|k) = X(k|k-1) + Kg(k) (Z(k) - H X(k|k-1)) ...(3)
Kg(k) = P(k|k-1) HT / (H P(k|k-1) HT + R) ...(4)
P(k|k) = (I - Kg(k) H) P(k|k-1) ...(5)

根據上述所提到對房間進行溫度推算的例子中,把房間看成是一個系統,然後對這個系統建立模型,當然模型不需要非常準確,因此可以假設預估該房間的溫度與前一時刻的溫度是相同的,所以 F = 1,沒有控制量,所以 U(k) = 0,因此:
X(k|k-1) = X(k-1|k-1) ...(6)

又因為 F = 1,所以 式(2) 可以改寫為:
P(k|k-1) = P(k-1|k-1) + Q ...(7)

也因為測量值是由溫度計取得,與溫度直接對應,所以 H = 1,式(3) ~ 式(5) 可以改寫為:
X(k|k) = X(k|k-1) + Kg(k) (Z(k) - X(k|k-1)) ...(8)
Kg(k) = P(k|k-1) / (P(k|k-1) + R) ...(9)
P(k|k) = (1 - Kg(k)) P(k|k-1) ...(10)




在 OpenCV 的實作程式碼為:

std::srand(std::time(0));

double Z[100] = {0};
for (int i = 0; i < 100; ++i) {
Z[i] = 19 + (std::rand() % 201) / 100.0; // 測量值 + 模擬雜訊: 19 ~ 21
}

double X = 0; // 狀態推算值
double P = 3; // 狀態推算值的共變異數
double F = 1; // 狀態轉換
double Q = 4; // 狀態預估模型的共變異數
double H = 1; // 測量值
double R = 4; // 測量值的共變異數矩陣
double I = 1;
double K = 0; // Kalman Gain

for (int i = 0; i < 100; ++i) {
X = X; // 預估值
P = P + Q; // 預估值雜訊
K = P / (P + R); // Kalman Gain
X = X + K * (Z[i] - X); // 推算值
P = (I - K) * P; // 推算值雜訊
}




結合小汽車實例

以小汽車來舉例,其參數如下:

狀態 xt 為 位置 與 速度:


控制量 ut 為 加速度,可以用來計算 xt


從上述公式可以看到輸出值為輸入值的線性組合,改寫為矩陣型式如下:


再簡化公式為:
xt- = Ftxt-1 + Btut ...(11)

F 為狀態轉移矩陣,表示如何從上一時刻的推算狀態預估出目前時刻的狀態
B 為控制矩陣,表示控制量 u 如何作用於目前時刻的狀態

在本例中,以共變異數矩陣 P 來表示該時刻的推算狀態的不確定性 - 也就是雜訊。
而狀態轉換矩陣 F 可以用來將不確定性傳遞到下一個時刻:
Pt- = F Pt-1 FT

除了將上一時刻的不確定性作傳遞之外,由於預測模型也會有不確定性(Q),所以在各個時刻其不確定性的傳遞關係須要修正為:
Pt- = F Pt-1 FT + Q ...(12)

假設利用紅外線測距儀來測量小汽車在每個時刻的位置(Zt),可以得到:
Zt = H xt + v

其中:
H 為 測量值矩陣, H = [1 0]
v 為 測量值的雜訊

另外測量值雜訊的共變異數矩陣以 R 表示。

有了測量值後,就可以對狀態值進行更新:
xt = xt- + Kt (zt - Hxt-) ...(13)

其中 Kt 為卡爾曼系數,公式為:
Kt = Pt HT (HPt-HT + R)-1 ...(14)

卡爾曼系數的主要作用有兩個:
  • 權衡預估值共變異數矩陣(P) 與 測量值共變異數矩陣(R) 兩個值之間的大小,來決定要比較相信 預測模型 還是 測量模型。
  • 將 測量值 的型式轉換為 預測值狀態 的型式,如此才能對預測值作修正。

最後一個步驟為更新推算狀態的雜訊分佈,供下一次迭代使用:
Pt = (I - KtH) P-1 ...(15)

在 matlab 的實作程式碼為:

Z = (1:100); % 測量值
noise = randn(1, 100); % 模擬測量值的雜訊
Z = Z + noise; % 測量值+雜訊

X = [0; 0]; % 狀態推算值
P = [1 0; 0 1]; % 狀態推算值的共變異數矩陣
F = [1 1; 0 1]; % 狀態轉換矩陣
Q = [0.0000001 0; 0 0.0000001]; % 狀態預估模型的共變異數矩陣
H = [1 0]; % 測量值矩陣
R = 1; % 測量值的共變異數(矩陣)

figure;
hold on;

for i = 1:100
X_ = F * X; % kalman filter eq. (1),缺少控制量 U
P_ = F * P * F' + Q; % kalman filter eq. (2)
K = P_ * H' / (H * P_ * H' + R); % kalman filter eq. (3)
X = X_ + K * (Z(i) - H * X); % kalman filter eq. (4)
P = (eye(2) - K * H) * P_; % kalman filter eq. (5)

plot(X(1), X(2));
end




在 OpenCV 的實作程式碼為:

std::srand(std::time(0));

float Z[100] = {0};
for (int i = 0; i < 100; ++i) {
Z[i] = i + (std::rand() % 100) / 100.0; // 測量值 + 模擬雜訊
}

cv::Mat X = (cv::Mat_<float>(2,1) << 0, 0); // 狀態推算值
cv::Mat P = (cv::Mat_<float>(2,2) << 1, 0, 0, 1); // 狀態推算值的共變異數矩陣
cv::Mat F = (cv::Mat_<float>(2,2) << 1, 1, 0, 1); // 狀態轉換矩陣
cv::Mat Q = (cv::Mat_<float>(2,2) << 0.0000001, 0, 0, 0.0000001); // 狀態預估模型的共變異數矩陣
cv::Mat H = (cv::Mat_<float>(1,2) << 1, 0); // 測量值矩陣
cv::Mat R = (cv::Mat_<float>(1,1) << 1); // 測量值的共變異數矩陣
cv::Mat I = (cv::Mat_<float>(2,2) << 1, 0, 0, 1);
cv::Mat K = (cv::Mat_<float>(2,1) << 0, 0);

for (int i = 0; i < 100; ++i) {
X = F * X; // 預估值
P = F * P * F.t() + Q; // 預估值雜訊
K = P * H.t() / ((cv::Mat)(H * P * H.t() + R)).at<float>(0,0); // Kalman Gain
X = X + K * (Z[i] - H * X); // 推算值
P = (I - K * H) * P; // 推算值雜訊
}




數學補充

共變異數(Covariance) 在機率論和統計學中用於衡量兩個變量的總體誤差。
期望值(平均數)分別為 E(X) = μ 與 E(Y) = v 的兩個實數隨機變量 X 與 Y 之間的共變異數定義為:
cov(X,Y) = E((X - μ) (Y - v)) = E(XY) - μv

在統計學與機率論中,共變異數矩陣(也稱離差矩陣、變異數-共變異數矩陣)是一個矩陣,其 i, j 位置的元素是第 i 個與第 j 個隨機向量(即隨機變量構成的向量)之間的共變異數。

變異數(Variance) 是共變異數的一種特殊情況,即當兩個變量是相同的情況,定義為:
Var(x) = E[(X - μ)2]
又因為 μ = E[X],可以展開成為:
Var(x) = E[X2 - 2XE[x] + (E[X])2] = E[X2] - 2E[X]E[X] + (E[X])2 = E[X2] - (E[X])2
上述的表示式可以計為:平方值的平均減去平均的平方

標準差(Standard Deviation, SD) 是一組數值自平均值分散開來的程度的一種測量觀念,數學符號σ,定義為變異數的算術平方根,反映組內個體間的離散程度;標準差與期望值之比為標準離差率。




Kalman Filter 相關文章

Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation

卡爾曼濾波 (Kalman Filter)




Reference

卡爾曼濾波器(Kalman Filter) 說明與介紹
卡爾曼濾波器的原理以及在 matlab 中的實現
Kalman Filter with MATLAB CODE
How a Kalman filter works, in pictures
The Extended Kalman Filter: An Interactive Tutorial for Non-Experts

文字內容 或 影像內容 部份參考、引用自網路,如有侵權,請告知,謝謝。
創作者介紹

拾人牙慧點滴

silverwind1982 發表在 痞客邦 PIXNET 留言(0) 人氣()