卡尔曼滤波介绍

- 📖 **卡尔曼滤波原理简介**
- - 🔑 **核心思想**
  - 📦 **卡尔曼滤波的组成**
- 🔍 **代码分析（`kalman_filter.py`）**
- - 🏗️ 1. 状态空间定义
  - 🔄 2. 初始化模型矩阵
  - 🚀 3. 核心方法分析
  - - 🧩 **预测**
    - 🧩 **更新**
    - 📏 **门控距离（gating distance）**
- 🧑‍💻 **示例代码：模拟目标运动**
- ✅ **小结**

📖 卡尔曼滤波原理简介

卡尔曼滤波（Kalman Filter, KF）是一种线性最优估计算法，用于通过一系列带噪声的观测值估计动态系统的状态。它假设系统模型和观测模型都是线性高斯的。

🔑 核心思想

预测 (Prediction)
根据系统的动态模型预测下一个时刻的状态和协方差。

$x^k∣k−1=Fx^k−1∣k−1+Buk\hat{x}_{k|k-1} = F \hat{x}_{k-1|k-1} + Bu_k$

$P_{k|k-1} = F P_{k-1|k-1} F^T + Q$
- $F$ ：状态转移矩阵
- $B$ ：控制输入矩阵
- $Q$ ：过程噪声协方差矩阵
- $P$ ：状态协方差矩阵
更新 (Update)
利用当前观测值修正预测结果。

卡尔曼增益：

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

更新状态：

$x^k∣k=x^k∣k−1+Kk(zk−Hx^k∣k−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 = LL^T$ ，从而为后续的线性方程组求解提供高效支持。
scipy.linalg.cho_solve 是 SciPy 中用于高效求解基于 Cholesky 分解的线性方程组的函数。它专门用于处理对称正定（或 Hermitian 正定）矩阵的线性方程组 $A x = 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)  # 马氏距离平方

计算状态预测与观测的马氏距离 $d2=(z−Hx^)TS−1(z−Hx^)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()