前言

第一章 什麼是濾波?

濾波是信號處理中的一個基本概念,其目的是從觀測信號中分離出有用的資訊,去除不必要的干擾和噪聲。常見的濾波方法包括:

1. 平滑濾波器:利用時間域上的相鄰觀測值進行加權平均,減小噪聲影響。如移動平均濾波器。
2. 頻率域濾波器:利用頻譜特性,保留目標頻帶的信號成分,濾除其他頻帶的干擾。如低通濾波器、高通濾波器。
3. 自適應濾波器:能夠自動調整濾波參數,以適應信號特性的變化。如LMS自適應濾波器。

這些濾波方法都有各自的優缺點,適用於不同的信號處理場景。其中,卡爾曼濾波是一種基於狀態空間模型的優化濾波算法,具有獨特的優勢。

1.1 濾波的基本原理

濾波的基本原理是利用信號的統計特性,對原始信號進行變換或處理,從而達到以下目的:

1. 去除噪聲:從觀測信號中去除各種干擾噪聲,提高信噪比。
2. 平滑處理:消除信號中的毛刺和尖峰,使信號變化更加平滑連續。
3. 頻帶選擇:選擇性地保留某些頻帶成分,濾除其他頻帶干擾。
4. 預測和估計:根據歷史資料預測未來信號走勢,或估計系統狀態變數。

1.2 常見的濾波方法

常見的濾波方法主要包括:

1. 平滑濾波器:利用時間域上的相鄰觀測值進行加權平均,減小噪聲影響。如移動平均濾波器。
2. 頻率域濾波器:利用頻譜特性,保留目標頻帶的信號成分,濾除其他頻帶的干擾。如低通濾波器、高通濾波器。
3. 自適應濾波器:能夠自動調整濾波參數,以適應信號特性的變化。如LMS自適應濾波器。
4. 狀態空間濾波器:基於系統的狀態空間模型,利用測量值和模型預測值進行最佳化融合,如卡爾曼濾波。

這些濾波方法都有各自的優缺點,適用於不同的信號處理場景。

1.3 移動平均濾波器的簡單案例

假設我們需要精確測量一個金條的重量。由於各種因素的影響,測量值可能會存在一些噪音或波動。為了提高測量的精度和可靠性,我們可以使用濾波器來處理原始測量數據。

在這個案例中,我們使用了移動平均濾波器(Moving Average Filter)。這種濾波器的原理是對當前測量值及其前後一定數量的值求平均,從而減少了由於測量噪音導致的波動。

具體實現步驟如下:

1. 模擬金條的真實重量和測量噪音。
2. 進行多次測量,獲得原始測量值。
3. 應用移動平均濾波器,對原始測量值進行平滑處理。
4. 比較原始測量值和經過濾波的測量值,觀察濾波效果。

實現代碼如下

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

import numpy as np

# 模擬金條重量測量
true_weight = 100.0  # 金條的真實重量
noise_std = 0.1      # 測量噪音標準差

# 進行10次測量
raw_weights = true_weight + np.random.normal(0, noise_std, 10)

# 應用移動平均濾波器
window_size = 3
filtered_weights = np.convolve(raw_weights, np.ones(window_size)/window_size, mode='valid')

# 輸出結果
print("Raw measurements:", raw_weights)
print("Filtered measurements:", filtered_weights)
print("True weight:", true_weight)

通過這種方法,可以有效地抑制噪音,得到更加準確的測量結果。這種濾波技術在需要高精度測量的應用場景中很常見,例如精密儀器、工業製造等領域。

第二章 什麼是貝葉斯濾波?

貝葉斯濾波器是一種基於貝葉斯定理的技術，用來估計動態系統的狀態。它特別適合處理不確定性和噪聲，廣泛應用於信號處理、機器學習和機器人導航等領域。

2.1 基本概念

• 先驗概率（Prior Probability）
  初始狀態的估計，是在獲取新數據之前對系統狀態的信念。先驗概率反映了對系統的初步了解。

• 似然函數（Likelihood Function）
  給定某一狀態，觀察到某項新數據或觀測值的概率。似然函數用來衡量觀測數據與假設狀態之間的匹配程度。

• 後驗概率（Posterior Probability）
  結合先驗概率和似然函數，更新後得到的系統狀態估計。後驗概率代表在觀測新數據後對系統狀態的最新信念。

2.2 數學推導

貝葉斯定理的數學表達式為：

$$P(X|Y) = \frac{P(Y|X) \cdot P(X)}{P(Y)}$$

在貝葉斯濾波器中：

• 狀態轉移模型
  預測下一狀態 $X_t$ 的先驗概率：

  $$P(X_t|Y_{1:t-1}) = \int P(X_t|X_{t-1}) \cdot P(X_{t-1}|Y_{1:t-1}) \, dX_{t-1}$$

  這表示根據先前狀態 $ X_{t-1} $ 和狀態轉移模型 $ P(X_t|X_{t-1}) $ 預測當前狀態。

• 更新步驟
  根據新觀測 $Y_t$ 更新後驗概率：

  $$P(X_t|Y_{1:t}) = \frac{P(Y_t|X_t) \cdot P(X_t|Y_{1:t-1})}{P(Y_t|Y_{1:t-1})}$$

  其中，證據 $P(Y_t|Y_{1:t-1})$ 可以通過如下公式計算：

  $$P(Y_t|Y_{1:t-1}) = \int P(Y_t|X_t) \cdot P(X_t|Y_{1:t-1}) \, dX_t$$

2.3 工作流程

1. 初始化
   設定系統狀態的初始先驗概率，這可能基於歷史數據或專家知識。

2. 預測步驟
   使用系統模型預測下一時刻的狀態，生成預測的先驗概率。

3. 更新步驟
   當獲得新觀測數據時，計算似然函數並更新後驗概率。

4. 迭代過程
   不斷重複預測和更新步驟，以持續改進狀態估計。

2.4 優勢

• 處理不確定性：能夠有效整合不確定性和噪聲。
• 適應性強：隨著新數據的到來，不斷更新狀態估計。
• 多領域應用：適用於各種動態系統，包括信號處理、金融市場分析和自動駕駛等。

2.5 實際應用

• 機器人導航：用於估計機器人的位置和運動狀態。
• 信號處理：在噪聲環境中提取有用信號。
• 經濟預測：分析金融數據以預測市場走勢。

2.6 簡單例子

假設有一個一維運動模型，我們想估計物體的位置。狀態轉移和觀測都受到噪聲影響。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

import numpy as np
import matplotlib.pyplot as plt

# 模擬運動和觀測
np.random.seed(0)
true_position = np.cumsum(np.random.randn(50))  # 真實位置
observations = true_position + np.random.randn(50) * 2  # 觀測值

# 初始化
predicted_position = [0]
predicted_uncertainty = [1]

# 參數
process_variance = 1  # 過程噪聲
measurement_variance = 4  # 觀測噪聲

# Kalman 濾波
for i in range(1, len(observations)):
    # 預測步驟
    predicted_position.append(predicted_position[i-1])
    predicted_uncertainty.append(predicted_uncertainty[i-1] + process_variance)
    
    # 更新步驟
    kalman_gain = predicted_uncertainty[i] / (predicted_uncertainty[i] + measurement_variance)
    predicted_position[i] += kalman_gain * (observations[i] - predicted_position[i])
    predicted_uncertainty[i] *= (1 - kalman_gain)

# 繪圖
plt.plot(true_position, label='True Position')
plt.plot(observations, label='Observations', linestyle='dotted')
plt.plot(predicted_position, label='Kalman Filter Prediction', linestyle='dashed')
plt.legend()
plt.xlabel('Time')
plt.ylabel('Position')
plt.show()

第三章 什麼是卡爾曼濾波?

3.1 卡爾曼濾波簡介

卡爾曼濾波（Kalman Filter）是一種廣泛應用於動態系統中的線性最小均方誤差估計算法。它於1960年由魯道夫·E·卡爾曼提出，旨在通過對系統狀態的動態模型和測量數據進行不斷更新，來估計系統的真實狀態。卡爾曼濾波的核心思想是利用現有的信息（如系統模型和測量數據），在每個時間步長上對狀態進行預測和更新，以實現最優估計。

卡爾曼濾波廣泛應用於導航、跟蹤、信號處理和控制系統等領域。例如，在GPS導航中，它可以根據衛星信號和系統模型來估計和校正位置和速度；在跟蹤系統中，它能夠在噪聲干擾下精確估計目標的運動狀態。

3.2 數學推導

卡爾曼濾波的數學推導主要包括兩個步驟：預測步驟（Prediction Step）和更新步驟（Update Step）。

1. 預測步驟

在這一步，根據上一狀態估計和系統模型來預測當前狀態。狀態方程和測量方程可以分別表示為：

$$x_{k} = A_{k-1} x_{k-1} + B_{k-1} u_{k-1} + w_{k-1}$$

$$z_{k} = H_{k} x_{k} + v_{k}$$

其中，$x_k$ 是系統狀態，$u_k$ 是控制輸入，$z_k$ 是測量值，$A_k$ 是狀態轉移矩陣，$B_k$ 是控制矩陣，$H_k$ 是測量矩陣，$w_k$ 和 $v_k$ 分別是過程噪聲和測量噪聲，通常假設它們是零均值高斯白噪聲。

根據上述模型，預測當前狀態和誤差協方差矩陣：

$$\hat{x}_{k|k-1} = A_{k-1} \hat{x}_{k-1|k-1} + B_{k-1} u_{k-1}$$

$$P_{k|k-1} = A_{k-1} P_{k-1|k-1} A_{k-1}^T + Q_{k-1}$$

其中，$\hat{x}_{k|k-1}$ 是預測的狀態估計，$P_{k|k-1}$ 是預測的誤差協方差矩陣，$Q_k$ 是過程噪聲協方差矩陣。

2. 更新步驟

在這一步，根據新的測量值來更新狀態估計和誤差協方差矩陣：

首先，計算卡爾曼增益 $K_k$：

$$K_{k} = P_{k|k-1} H_{k}^T (H_{k} P_{k|k-1} H_{k}^T + R_{k})^{-1}$$

其中，$R_k$ 是測量噪聲協方差矩陣。

然後，更新狀態估計：

$$\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_{k} (z_{k} - H_{k} \hat{x}_{k|k-1})$$

最後，更新誤差協方差矩陣：

$$P_{k|k} = (I - K_{k} H_{k}) P_{k|k-1}$$

其中，$\hat{x}_{k|k}$ 是更新的狀態估計，$P_{k|k}$ 是更新的誤差協方差矩陣，$I$ 是單位矩陣。

通過不斷重複這兩個步驟，卡爾曼濾波可以在動態系統中提供連續的狀態估計，並且能夠有效地抑制噪聲對估計結果的影響。

第四章 什麼是粒子濾波?

4.1 粒子濾波簡介

粒子濾波（Particle Filter）是一種基於蒙特卡洛方法的非線性、非高斯狀態估計技術。它也被稱為序列蒙特卡洛（Sequential Monte Carlo, SMC）方法。與卡爾曼濾波不同，粒子濾波能夠處理任意形式的狀態轉移和觀測模型，這使得它在應對複雜系統和非高斯噪聲環境下具有很強的靈活性和適應性。

粒子濾波的基本思想是使用大量的隨機樣本（即粒子）來表示狀態的概率分佈。每個粒子代表一個可能的狀態，並具有對應的權重。通過對這些粒子的狀態進行預測和更新，粒子濾波能夠逐步逼近狀態的真實概率分佈。

4.2 粒子濾波的數學推導

粒子濾波的主要步驟包括初始化、預測、更新和重采樣：

1. 初始化

   在初始化階段，根據狀態的初始分佈生成一組粒子。每個粒子被賦予相同的初始權重。

   $$\{ x_k^{(i)}, w_k^{(i)} \}_{i=1}^N$$

   其中，$x_k^{(i)}$ 表示第 $i$ 個粒子的狀態，$w_k^{(i)}$ 表示其對應的權重，$N$ 是粒子的數量。

2. 預測

   在預測步驟中，根據狀態轉移模型對每個粒子的狀態進行預測。狀態轉移模型可以表示為：

   $$x_{k+1}^{(i)} = f(x_k^{(i)}, u_k) + v_k^{(i)}$$

   其中，$f(\cdot)$ 是狀態轉移函數，$u_k$ 是控制輸入，$v_k^{(i)}$ 是過程噪聲。

3. 更新

   在更新步驟中，根據觀測模型計算每個粒子的權重。觀測模型可以表示為：

   $$w_{k+1}^{(i)} \propto p(z_{k+1} | x_{k+1}^{(i)})$$

   其中，$p(z_{k+1} | x_{k+1}^{(i)})$ 是觀測模型的似然函數，$ z_{k+1} $ 是新的觀測值。

4. 重采樣

   重采樣步驟旨在解決粒子退化問題，即隨著時間推移，少數粒子的權重會越來越大，而大部分粒子的權重會趨近於零。通過重采樣，可以從當前粒子集中抽取新的粒子，並賦予這些新粒子相等的權重，以保持粒子的多樣性。

   重采樣的過程如下：

   $$\{ x_k^{(i)}, w_k^{(i)} \}_{i=1}^N \rightarrow \{ x_k^{(j)}, \frac{1}{N} \}_{j=1}^N$$

   其中，新粒子 $x_k^{(j)}$ 是根據當前粒子的權重進行抽樣的結果。

通過不斷重複上述步驟，粒子濾波可以在動態系統中提供連續的狀態估計，並能夠有效地應對非線性和非高斯問題。粒子濾波在定位、導航、機器人、信號處理等領域有廣泛的應用。