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

卡爾曼濾波器不是硬體，而是一套**「結合數學預測與感測器測量，找出最優解」**的演算法。 
 1. 白話情境：開車過隧道 (GPS 雜訊 vs. 時速表) 
 想像你正在開車，你想知道自己「精確的位置」。你手上有兩個資訊來源： 
 
 你的油門與時速表 (數學預測 / 物理模型)： 你知道自己前一秒在哪，也知道現在時速 60 km/h，所以你可以「猜」出自己現在大概在哪。但如果剛好遇到逆風或上坡，這個預測就不太準。 
 車上的 GPS (感測器測量)： GPS 會告訴你座標，但它有雜訊，可能會有 $\pm 5$ 公尺的誤差。 
 
 卡爾曼濾波器在做什麼？ 
當你開進隧道（GPS 訊號變弱，雜訊超大），它會選擇 相信你的時速表預測多一點 ；當你開在空曠的高速公路（GPS 超準），它會選擇 相信 GPS 多一點 。
它透過一個叫做**「卡爾曼增益 (Kalman Gain)」**的比例，不斷在這兩者之間抓平衡，算出一個比單看 GPS 或單看時速表都還要準確的「真實位置」。 
 
 2. 核心運作邏輯：預測 (Predict) 與 更新 (Update) 
 卡爾曼濾波器永遠在做這兩件事的無限迴圈： 
 
 預測 (Predict)「看未來」： 根據上一秒的狀態和物理公式，猜測現在的狀態。同時也會估算這次「猜測的誤差有多大」。 
 更新 (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;