📖 卡爾曼濾波原理簡介

卡爾曼濾波（Kalman Filter, KF）是一種線性最優估計算法，用于通過一系列帶噪聲的觀測值估計動態系統的狀態。它假設系統模型和觀測模型都是線性高斯的。

🔑 核心思想

1. 預測 (Prediction)
   根據系統的動態模型預測下一個時刻的狀態和協方差。

   $${\hat{x}}_{k|k?1}=F{\hat{x}}_{k?1|k?1}+B{u}_k$$

   $$P_{k|k?1}=F{P}_{k?1|k?1}{F}^T+Q$$

   • $F$：狀態轉移矩陣
   • $B$：控制輸入矩陣
   • $Q$：過程噪聲協方差矩陣
   • $P$：狀態協方差矩陣

2. 更新 (Update)
   利用當前觀測值修正預測結果。

   卡爾曼增益：

   $$K_k=P_{k|k?1}{H}^T(H{P}_{k|k?1}{H}^T+R{)}^{?1}$$

   更新狀態：

   $${\hat{x}}_{k|k}={\hat{x}}_{k|k?1}+K_k(z_k?H{\hat{x}}_{k|k?1})$$

   更新協方差：

   $$P_{k|k}=(I?K_{k}H)P_{k|k?1}$$

   • $H$：觀測矩陣
   • $R$：觀測噪聲協方差矩陣
   • $z_k$：觀測向量

📦 卡爾曼濾波的組成

模塊	功能描述
狀態預測	根據動態模型預測當前狀態
觀測預測	將狀態預測結果投影到觀測空間
計算卡爾曼增益	結合預測和觀測權重，平衡兩者的置信度
狀態更新	利用觀測值修正預測
協方差更新	更新估計的不確定性

---

🔍 代碼分析（kalman_filter.py）

這個代碼是一個簡化版的卡爾曼濾波實現，用于目標跟蹤（tracking bounding boxes in image space）。
代碼參考實現：Yolov5-deepsort-inference/deep_sort/deep_sort/sort/kalman_filter.py

🏗? 1. 狀態空間定義

# 狀態空間 (x, y, a, h, vx, vy, va, vh)
# x, y : 邊框中心
# a    : 寬高比
# h    : 高度
# vx, vy, va, vh : 對應的速度
# 8維狀態向量
x = [x, y, a, h, vx, vy, va, vh] 

8維狀態向量，前4維是位置+形狀，后4維是速度。

---

🔄 2. 初始化模型矩陣

def __init__(self):# 運動矩陣Fndim, dt = 4, 1.self._motion_mat = np.eye(8)for i in range(4):self._motion_mat[i, 4 + i] = dt# 觀測矩陣Hself._update_mat = np.eye(ndim, 2 * ndim)self._std_weight_position = 1. / 20self._std_weight_velocity = 1. / 160# 恒定速度模型（dt=1）# _motion_mat = [#     [1,0,0,0,1,0,0,0],  # x = x + vx#     [0,1,0,0,0,1,0,0],  # y = y + vy#     [0,0,1,0,0,0,1,0],  # a = a + va#     [0,0,0,1,0,0,0,1],  # h = h + vh#     [0,0,0,0,1,0,0,0],  # vx不變#     ...                  # 其余速度項同理# ]# _update_mat = [#     [1,0,0,0,0,0,0,0], #     [0,1,0,0,0,0,0,0],  #     [0,0,1,0,0,0,0,0], #     [0,0,0,1,0,0,0,0],  # ]def initiate(self, measurement):mean_pos = measurement  # [x, y, a, h]mean_vel = np.zeros_like(mean_pos)  # 速度初始化為0mean = np.r_[mean_pos, mean_vel]  # 8維狀態向量# 協方差初始化（與目標高度h相關）std = [2 * self._std_weight_position * h,2 * self._std_weight_position * h,1e-2,  # 寬高比噪聲較小2 * self._std_weight_position * h,10 * self._std_weight_velocity * h,10 * self._std_weight_velocity * h,1e-5,  # 寬高比速度噪聲極小10 * self._std_weight_velocity * h]covariance = np.diag(np.square(std))  # 對角協方差矩陣 Preturn mean, covariance

• 運動矩陣 $F$：假設目標勻速移動。
• 觀測矩陣 $H$：只觀測位置和形狀 (x, y, a, h)，它固定不變是因為觀測模型假設始終相同。如果觀測維度、傳感器模型或觀測函數發生改變，就需要讓$H$隨時間更新。
• 位置噪聲權重（1/20） > 速度噪聲權重（1/160）。
• 高度h作為尺度因子：大目標位置更確定，小目標位置更不確定。
• 寬高比a及其速度的噪聲極小（1e-2, 1e-5），因目標比例通常穩定。

---

🚀 3. 核心方法分析

🧩 預測

# def predict(self, mean, covariance):
#     mean = F * mean
#     covariance = F * covariance * F^T + Qdef predict(self, mean, covariance):# 過程噪聲（依賴目標高度）std_pos = [self._std_weight_position * h, ...]std_vel = [self._std_weight_velocity * h, ...]motion_cov = np.diag(np.square(np.r_[std_pos, std_vel]))# 狀態預測  x_k = F * x_{k-1}mean = np.dot(self._motion_mat, mean) # 協方差預測   P_k = F * P_{k-1} * F^T + Qcovariance = np.linalg.multi_dot((self._motion_mat, covariance, self._motion_mat.T)) + motion_covreturn mean, covariance

• 預測當前狀態
• 加入過程噪聲 motion_cov

---

🧩 更新

# 投影（觀測空間預測）， 計算觀測預測及其不確定性
# def project(self, mean, covariance):
#     mean = H * mean
#     covariance = H * covariance * H^T + R# def update(self, mean, covariance, measurement):
#     projected_mean, projected_cov = self.project(mean, covariance)
#     kalman_gain = covariance * H^T * (projected_cov)^-1
#     innovation = measurement - projected_mean
#     new_mean = mean + kalman_gain * innovation
#     new_covariance = covariance - kalman_gain * projected_cov * kalman_gain^Tdef project(self, mean, covariance):# 觀測噪聲（僅位置相關）std = [self._std_weight_position * mean[3],self._std_weight_position * mean[3],1e-1,self._std_weight_position * mean[3]]innovation_cov = np.diag(np.square(std))  # 觀測噪聲R# 投影到觀測空間：z = H * xmean = np.dot(self._update_mat, mean)  # 觀測協方差：H * P * H^T + Rcovariance = np.linalg.multi_dot((self._update_mat, covariance, self._update_mat.T)) + innovation_covreturn mean, covariance + innovation_covdef update(self, mean, covariance, measurement):# 1. 預測觀測空間分布projected_mean, projected_cov = self.project(mean, covariance)# 2. 計算卡爾曼增益（Cholesky分解加速）chol_factor = scipy.linalg.cho_factor(projected_cov, lower=True, check_finite=False)kalman_gain = scipy.linalg.cho_solve(chol_factor, np.dot(covariance, self._update_mat.T).T).T# 3. 融合觀測innovation = measurement - projected_mean  # 新息new_mean = mean + np.dot(innovation, kalman_gain.T)# 4. 更新協方差new_covariance = covariance - np.linalg.multi_dot((kalman_gain, projected_cov, kalman_gain.T))

• 根據觀測修正預測。
• scipy.linalg.cho_factor 是 SciPy 中用于計算矩陣的 Cholesky 分解的函數，專門針對對稱（或 Hermitian）正定矩陣。它的核心作用是將矩陣分解為下三角矩陣 L 和其共軛轉置 $L^T$ （或 $L^H$ ）的乘積形式 $A=L{L}^T$，從而為后續的線性方程組求解提供高效支持。
• scipy.linalg.cho_solve 是 SciPy 中用于高效求解基于 Cholesky 分解的線性方程組的函數。它專門用于處理對稱正定（或 Hermitian 正定）矩陣的線性方程組 $Ax=b$ ，通過結合 cho_factor 的分解結果，顯著提升計算效率。

---

📏 門控距離（gating distance）

• 馬氏距離 (Mahalanobis distance) 測量預測和觀測的差異，用于在跟蹤多目標時判斷某觀測是否匹配預測。

"""
Table for the 0.95 quantile of the chi-square distribution with N degrees of
freedom (contains values for N=1, ..., 9). Taken from MATLAB/Octave's chi2inv
function and used as Mahalanobis gating threshold.
"""
chi2inv95 = {1: 3.8415,2: 5.9915,3: 7.8147,4: 9.4877,5: 11.070,6: 12.592,7: 14.067,8: 15.507,9: 16.919}def gating_distance(self, mean, covariance, measurements, only_position=False):# 投影到觀測空間mean, cov = self.project(mean, covariance)if only_position:  # 僅使用(x,y)mean, cov = mean[:2], cov[:2, :2]measurements = measurements[:, :2]# 計算馬氏距離（高效三角分解）cholesky_factor = np.linalg.cholesky(cov)d = measurements - meanz = scipy.linalg.solve_triangular(cholesky_factor, d.T, lower=True)return np.sum(z * z, axis=0)  # 馬氏距離平方

• 計算狀態預測與觀測的馬氏距離$d^2=(z?H\hat{x}{)}^T{S}^{?1}(z?H\hat{x})$

• 通過chi2inv95表（卡方分布95%置信區間）設定閾值，用于數據關聯

• np.linalg.cholesky 和 scipy.linalg.solve_triangular 在求解線性方程組時，需顯式調用兩次 solve_triangular。
  補充代碼對比：

  # =======================cholesky + solve_triangular=======================
  import numpy as np
  from scipy.linalg import solve_triangularA = np.array([[4, 12, -16], [12, 37, -43], [-16, -43, 98]])  # 對稱正定矩陣
  b = np.array([10, 8, 3])# 分解階段
  L = np.linalg.cholesky(A)  # 返回下三角矩陣 L# 求解階段
  y = solve_triangular(L, b, lower=True)   # 解 Ly = b（前向替換）
  x = solve_triangular(L.T, y, lower=False)  # 解 L^T x = y（后向替換）# =======================cho_factor + cho_solve=======================
  from scipy.linalg import cho_factor, cho_solvec, low = cho_factor(A)  # 分解并返回壓縮格式的因子矩陣
  x = cho_solve((c, low), b)  # 直接求解 Ax = b
  

---

🧑?💻 示例代碼：模擬目標運動

下面的代碼用你的 KalmanFilter 進行一個簡單演示：

import numpy as np
import matplotlib.pyplot as plt
from kalman_filter import KalmanFilterkf = KalmanFilter()
true_positions = []
estimated_positions = []# 模擬真實軌跡 (勻速直線)
true_state = np.array([0, 0, 1, 100, 1, 1, 0, 0])
measurements = []
for i in range(50):true_state[:4] += true_state[4:]  # 更新真實位置true_positions.append(true_state[:2].copy())# 添加觀測噪聲measurement = true_state[:4] + np.random.randn(4) * 5measurements.append(measurement)# 初始化
mean, covariance = kf.initiate(measurements[0])# 濾波
for z in measurements:mean, covariance = kf.predict(mean, covariance)mean, covariance = kf.update(mean, covariance, z)estimated_positions.append(mean[:2].copy())# 繪圖
true_positions = np.array(true_positions)
estimated_positions = np.array(estimated_positions)
measurements = np.array(measurements)plt.plot(true_positions[:, 0], true_positions[:, 1], 'g-', label="True Path")
plt.plot(measurements[:, 0], measurements[:, 1], 'rx', label="Measurements")
plt.plot(estimated_positions[:, 0], estimated_positions[:, 1], 'b-', label="Kalman Filter")
plt.legend()
plt.title("Kalman Filter Tracking Example")
plt.show()

---

? 小結

這份代碼實現了標準卡爾曼濾波，適配于目標跟蹤任務：

• 預測：估計下一個位置
• 更新：用新觀測修正預測
• 門控距離：多目標關聯時使用