Skip to main content

卡爾曼濾波器 (Kalman Filter) 白話文與實戰

卡爾曼濾波器不是硬體，而是一套**「結合數學預測與感測器測量，找出最優解」**的演算法。

1. 白話情境：開車過隧道 (GPS 雜訊 vs. 時速表)

想像你正在開車，你想知道自己「精確的位置」。你手上有兩個資訊來源：

  1. 你的油門與時速表 (數學預測 / 物理模型)： 你知道自己前一秒在哪，也知道現在時速 60 km/h，所以你可以「猜」出自己現在大概在哪。但如果剛好遇到逆風或上坡，這個預測就不太準。
  2. 車上的 GPS (感測器測量)： GPS 會告訴你座標，但它有雜訊，可能會有 $\pm 5$ 公尺的誤差。

卡爾曼濾波器在做什麼？ 當你開進隧道（GPS 訊號變弱，雜訊超大），它會選擇相信你的時速表預測多一點；當你開在空曠的高速公路（GPS 超準），它會選擇相信 GPS 多一點。 它透過一個叫做**「卡爾曼增益 (Kalman Gain)」**的比例，不斷在這兩者之間抓平衡，算出一個比單看 GPS 或單看時速表都還要準確的「真實位置」。

2. 核心運作邏輯：預測 (Predict) 與 更新 (Update)

卡爾曼濾波器永遠在做這兩件事的無限迴圈：

  1. 預測 (Predict)「看未來」： 根據上一秒的狀態和物理公式，猜測現在的狀態。同時也會估算這次「猜測的誤差有多大」。
  2. 更新 (Update)「面對現實」： 拿到感測器熱騰騰的數據後，把「猜測值」跟「測量值」做加權平均（權重就是卡爾曼增益）。

3. 卡爾曼濾波器的 5 條神聖方程式

雖然看起來很可怕，但只要知道它們對應的意義，寫程式時照抄即可。這裡使用的是矩陣形式（因為通常會同時計算位置、速度等多個變數）。

➡️ 步驟一：預測 (Predict)

  • 狀態預測 (猜測現在位置)： $$\hat{x}{k}^{-} = A \hat{x}{k-1} + B u_k$$
  • 誤差協方差預測 (猜測的不確定性有多大)： $$P_{k}^{-} = A P_{k-1} A^T + Q$$ (註：$Q$ 代表系統模型本身的雜訊，也就是你對物理公式的不信任程度)

➡️ 步驟二：更新 (Update)

  • 計算卡爾曼增益 (決定要相信模型還是感測器)： $$K_k = P_{k}^{-} H^T (H P_{k}^{-} H^T + R)^{-1}$$ (註：$R$ 代表感測器的測量雜訊，也就是你對儀器的不信任程度)
  • 狀態更新 (算出最終的最優估計值)： $$\hat{x}k = \hat{x}{k}^{-} + K_k (z_k - H \hat{x}_{k}^{-})$$
  • 誤差協方差更新 (更新當前的不確定性，留給下一回合用)： $$P_k = (I - K_k H) P_{k}^{-}$$

4. MATLAB 1D 實戰：過濾超晃的感測器雜訊

我們來寫一個最簡單的 1D 範例（純量，不用矩陣）。假設我們正在測量一個恆定電壓（真實值是 1.2V），但電壓表的雜訊非常大，看看卡爾曼濾波器怎麼把它壓平！

% 1. 基本設定
N = 100;                % 模擬 100 個時間點
true_voltage = 1.2;     % 真實電壓 (我們假裝不知道)
measured_v = true_voltage + 0.1 * randn(1, N); % 加入常態分佈雜訊的測量值

% 2. 初始化卡爾曼濾波器變數
x_est = 0;              % 初始猜測的狀態 (隨便猜一個 0)
P = 1;                  % 初始的不確定性 (設 1 代表很不確定)

% 兩個魔法調音旋鈕：Q 和 R
Q = 1e-5;               % 過程雜訊 (我們相信電壓是恆定的，所以設很小)
R = 0.01;               % 測量雜訊 (我們知道電壓表很爛，雜訊大概是 0.1^2)

% 準備陣列來存畫圖用的資料
kalman_result = zeros(1, N);

% 3. 開始卡爾曼濾波器迴圈 (Predict -> Update)
for k = 1:N
    
    % --- [預測階段 Predict] ---
    % 因為是測量恆定電壓，沒有控制力輸入，也沒有物理移動，所以 A=1, B=0
    x_pred = x_est;             % 預測下一個狀態跟現在一樣
    P_pred = P + Q;             % 預測不確定性增加一點點
    
    % --- [更新階段 Update] ---
    % 拿到最新的測量值 z_k
    z_k = measured_v(k);
    
    % 計算卡爾曼增益 (H=1)
    K = P_pred / (P_pred + R);
    
    % 更新最優估計狀態
    x_est = x_pred + K * (z_k - x_pred);
    
    % 更新不確定性
    P = (1 - K) * P_pred;
    
    % 存起來準備畫圖
    kalman_result(k) = x_est;
end

% 4. 畫圖比較
figure;
hold on;
plot(1:N, true_voltage * ones(1,N), 'g-', 'LineWidth', 2); % 真實值 (綠線)
plot(1:N, measured_v, 'r.', 'MarkerSize', 10);             % 測量值 (紅點)
plot(1:N, kalman_result, 'b-', 'LineWidth', 2);            % 卡爾曼濾波結果 (藍線)
hold off;

title('卡爾曼濾波器 1D 電壓測量實戰');
xlabel('時間步長');
ylabel('電壓 (V)');
legend('真實值', '感測器測量值 (含雜訊)', '卡爾曼濾波後的最優估計');
grid on;
Back to top