# Kalman濾波器的簡單實現是怎樣的
## 引言
Kalman濾波器是一種高效的遞歸濾波器，能夠從一系列包含噪聲的觀測數據中估計動態系統的狀態。它由Rudolf E. Kálmán在1960年提出，如今已廣泛應用于導航系統、信號處理、機器人、經濟學等領域。本文將詳細介紹Kalman濾波器的基本原理，并通過Python代碼展示其簡單實現過程。
---
## 1. Kalman濾波器的基本原理
### 1.1 狀態空間模型
Kalman濾波器基于線性動態系統的狀態空間模型，主要包括兩個方程：
1. **狀態方程（預測方程）**：
\[
x_k = A x_{k-1} + B u_k + w_k
\]
- \(x_k\)：系統在時刻\(k\)的狀態向量
- \(A\)：狀態轉移矩陣
- \(u_k\)：控制輸入
- \(w_k\)：過程噪聲（假設為高斯白噪聲，協方差矩陣為\(Q\)）
2. **觀測方程（更新方程）**：
\[
z_k = H x_k + v_k
\]
- \(z_k\)：觀測向量
- \(H\)：觀測矩陣
- \(v_k\)：觀測噪聲（假設為高斯白噪聲，協方差矩陣為\(R\)）
### 1.2 濾波器的兩個階段
Kalman濾波器通過以下兩個階段遞歸運行：
1. **預測階段**：
- 根據前一時刻的狀態估計當前狀態。
- 更新狀態協方差矩陣以反映預測的不確定性。
2. **更新階段**：
- 結合觀測數據修正預測值。
- 計算卡爾曼增益（Kalman Gain），決定預測值和觀測值的權重。
---
## 2. 一維Kalman濾波器的實現
以下是一個簡單的一維Kalman濾波器實現，用于估計勻速運動物體的位置。
### 2.1 初始化參數
```python
import numpy as np
# 初始狀態（位置和速度）
x = np.array([[0.0], [0.0]]) # [位置, 速度]
# 狀態轉移矩陣（假設勻速運動）
A = np.array([[1, 1], [0, 1]])
# 控制輸入矩陣（本例中無控制輸入）
B = np.array([[0], [0]])
# 觀測矩陣（僅觀測位置）
H = np.array([[1, 0]])
# 過程噪聲協方差
Q = np.array([[0.01, 0], [0, 0.01]])
# 觀測噪聲協方差
R = np.array([[1.0]])
# 初始狀態協方差
P = np.eye(2)
def kalman_predict(x, P, A, Q, B, u):
x_pred = A @ x + B @ u
P_pred = A @ P @ A.T + Q
return x_pred, P_pred
def kalman_update(x_pred, P_pred, z, H, R):
K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R)
x_updated = x_pred + K @ (z - H @ x_pred)
P_updated = (np.eye(2) - K @ H) @ P_pred
return x_updated, P_updated
# 生成真實位置（勻速運動）
true_velocity = 0.5
true_positions = [i * true_velocity for i in range(50)]
# 添加觀測噪聲
noisy_observations = [p + np.random.normal(0, 1) for p in true_positions]
# 運行Kalman濾波器
estimated_positions = []
for z in noisy_observations:
x, P = kalman_predict(x, P, A, Q, B, 0)
x, P = kalman_update(x, P, np.array([[z]]), H, R)
estimated_positions.append(x[0, 0])
import matplotlib.pyplot as plt
plt.figure(figsize=(10, 6))
plt.plot(true_positions, label="True Position", linestyle="--")
plt.plot(noisy_observations, label="Noisy Observations", marker="o", alpha=0.5)
plt.plot(estimated_positions, label="Kalman Estimate", marker="x")
plt.legend()
plt.xlabel("Time Step")
plt.ylabel("Position")
plt.title("1D Kalman Filter Demo")
plt.show()
輸出效果：
（圖中藍色虛線為真實位置，橙色點為含噪聲觀測值，綠色線為Kalman濾波結果）
對于更復雜的系統（如物體跟蹤），需擴展至多維狀態。以下是一個2D位置跟蹤的示例：
# 狀態向量：[x, y, vx, vy]
x = np.array([[0], [0], [0], [0]])
# 狀態轉移矩陣（假設勻速運動）
dt = 1.0 # 時間步長
A = np.array([
[1, 0, dt, 0],
[0, 1, 0, dt],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
# 僅觀測x和y坐標
H = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
])
非線性系統：
參數調優：
計算效率：
# 1D Kalman Filter完整實現
import numpy as np
import matplotlib.pyplot as plt
class SimpleKalmanFilter:
def __init__(self, initial_state, A, H, Q, R, P):
self.x = initial_state
self.A = A
self.H = H
self.Q = Q
self.R = R
self.P = P
def predict(self, u=0):
self.x = self.A @ self.x
self.P = self.A @ self.P @ self.A.T + self.Q
def update(self, z):
K = self.P @ self.H.T @ np.linalg.inv(self.H @ self.P @ self.H.T + self.R)
self.x = self.x + K @ (z - self.H @ self.x)
self.P = (np.eye(len(self.x)) - K @ self.H) @ self.P
# 使用示例
kf = SimpleKalmanFilter(
initial_state=np.array([[0], [0]]),
A=np.array([[1, 1], [0, 1]]),
H=np.array([[1, 0]]),
Q=np.array([[0.01, 0], [0, 0.01]]),
R=np.array([[1.0]]),
P=np.eye(2)
)
# 運行濾波
measurements = [i*0.5 + np.random.normal(0, 1) for i in range(50)]
estimates = []
for z in measurements:
kf.predict()
kf.update(np.array([[z]]))
estimates.append(kf.x[0, 0])
# 可視化
plt.plot(estimates, label="Estimate")
plt.plot(measurements, 'ro', label="Measurement")
plt.legend()
plt.show()
Kalman濾波器通過優雅的數學框架將預測與觀測數據融合，是狀態估計的強大工具。本文展示的一維實現僅是其最基礎形式，讀者可進一步探索其在更復雜場景（如傳感器融合、SLAM等）中的應用。關鍵點在于：
1. 正確建模系統動態
2. 合理設置噪聲參數
3. 理解矩陣運算的物理意義
擴展閱讀：
- 《Kalman Filtering: Theory and Practice》 by Mohinder S. Grewal
- 開源項目：Pythonfilterpy庫 “`
注：實際運行時需替換示例中的占位圖鏈接。文章可根據需要調整代碼細節或補充理論推導部分。
免責聲明：本站發布的內容（圖片、視頻和文字）以原創、轉載和分享為主，文章觀點不代表本網站立場，如果涉及侵權請聯系站長郵箱：is@yisu.com進行舉報，并提供相關證據，一經查實，將立刻刪除涉嫌侵權內容。
