KalmanFilter

卡尔曼滤波器（Kalman Filter，简称 KF）是一种基于状态空间模型的递归滤波算法，广泛应用于信号处理、控制工程、目标跟踪、导航等领域。它的主要作用是通过对噪声和不确定性进行建模，估计系统的状态，并且能在不完全或含噪的观测数据下，提供最优估计。

卡尔曼滤波器的核心原理

卡尔曼滤波器的核心思想是利用传感器的测量数据以及预测模型来递归地更新对当前系统状态的估计。它根据系统的动态模型（预测）和测量模型（更新）来调整状态估计，确保估计的最优性。

卡尔曼滤波器假设：

系统的动态模型是线性的。
系统的噪声和观测的噪声是高斯白噪声（即零均值，已知方差）。

卡尔曼滤波器的数学模型

卡尔曼滤波器包括两个主要步骤：预测（Prediction）*和*更新（Update）。

1. 状态预测

卡尔曼滤波器假设系统的状态在当前时间点 k 是由上一时间点 k-1 的状态通过某个线性模型预测得到的：

状态转移方程：

xk=A⋅xk−1+B⋅uk+wkx_k = A \cdot x_{k-1} + B \cdot u_k + w_k

xkx_k 是系统在时刻 k 的状态向量。
AA 是状态转移矩阵，描述了状态如何从上一时刻转移到当前时刻。
BB 是控制矩阵，表示控制输入对状态的影响。
uku_k 是控制输入向量。
wkw_k 是过程噪声（通常假设为零均值高斯噪声）。
预测协方差：

Pk=A⋅Pk−1⋅AT+QP_k = A \cdot P_{k-1} \cdot A^T + Q

PkP_k 是状态估计的协方差矩阵，描述了对状态的估计的不确定性。
QQ 是过程噪声协方差矩阵，描述了过程噪声的不确定性。

2. 更新阶段

在每个时间步，卡尔曼滤波器会使用观测值来更新当前的状态估计，减少估计的误差。

观测模型：

zk=H⋅xk+vkz_k = H \cdot x_k + v_k

zkz_k 是在时刻 k 观测到的测量值。
HH 是观测矩阵，将状态向量 xkx_k 转换为观测空间。
vkv_k 是观测噪声（通常假设为零均值高斯噪声）。
卡尔曼增益：

Kk=Pk⋅HT⋅(H⋅Pk⋅HT+R)−1K_k = P_k \cdot H^T \cdot (H \cdot P_k \cdot H^T + R)^{-1}

KkK_k 是卡尔曼增益，用来平衡预测和测量的影响。
RR 是观测噪声的协方差矩阵。
更新状态估计：

xk=xk+Kk⋅(zk−H⋅xk)x_k = x_k + K_k \cdot (z_k - H \cdot x_k)

xkx_k 是修正后的状态估计。
更新协方差：

Pk=(I−Kk⋅H)⋅PkP_k = (I - K_k \cdot H) \cdot P_k

PkP_k 是更新后的协方差矩阵，反映了状态估计的不确定性。

卡尔曼滤波器的流程

初始化：设定初始状态估计 x0x_0 和初始协方差矩阵 P0P_0。
预测阶段：根据系统的动态模型预测下一时刻的状态和协方差。
更新阶段：获取测量数据后，使用观测模型更新状态估计和协方差。
重复：不断进行预测和更新，直到所有数据处理完。

示例：卡尔曼滤波器的 Python 实现

假设我们要估计一个物体的位置和速度，且我们有一个加速度控制输入和位置的测量数据。我们假设物体的状态是 [位置, 速度]，而测量数据是 位置。

import numpy as np
import matplotlib.pyplot as plt

# 定义时间步长
dt = 1.0  # 时间间隔（秒）

# 状态转移矩阵 A
A = np.array([[1, dt],
              [0, 1]])

# 控制矩阵 B
B = np.array([[0.5 * dt**2],
              [dt]])

# 观测矩阵 H
H = np.array([[1, 0]])

# 过程噪声协方差矩阵 Q
Q = np.array([[0.25 * dt**4, 0.5 * dt**3],
              [0.5 * dt**3, dt**2]])

# 观测噪声协方差矩阵 R
R = np.array([[1]])

# 初始状态和协方差
x = np.array([[0], [0]])  # 初始状态 [位置, 速度]
P = np.eye(2)  # 初始协方差矩阵

# 控制输入（假设加速度为恒定值）
u = np.array([[0.1]])  # 假设加速度为 0.1 m/s^2

# 测量数据（假设每次测量到的位置）
measurements = [1, 2.2, 3.1, 4.2, 5.1, 6.0, 7.0, 8.1]

# 卡尔曼滤波器迭代
estimated_positions = []
for z in measurements:
    # 预测阶段
    x = A @ x + B @ u  # 状态预测
    P = A @ P @ A.T + Q  # 协方差预测

    # 更新阶段
    y = z - H @ x  # 测量残差
    S = H @ P @ H.T + R  # 测量预测协方差
    K = P @ H.T @ np.linalg.inv(S)  # 卡尔曼增益

    x = x + K @ y  # 更新状态
    P = (np.eye(2) - K @ H) @ P  # 更新协方差

    # 记录估计的位置
    estimated_positions.append(x[0, 0])

# 绘制结果
plt.plot(measurements, label='Measured Position')
plt.plot(estimated_positions, label='Estimated Position')
plt.legend()
plt.xlabel('Time Step')
plt.ylabel('Position')
plt.show()

对于非线性系统，经典的 卡尔曼滤波器（Linear Kalman Filter）由于其假设系统是线性的，不能直接应用。为了解决这个问题，卡尔曼滤波器有两种扩展形式：扩展卡尔曼滤波器（EKF，Extended Kalman Filter） 和 无迹卡尔曼滤波器（UKF，Unscented Kalman Filter）。这两种方法都能处理非线性系统，并且在实际应用中非常常见。

1. 扩展卡尔曼滤波器（EKF）

扩展卡尔曼滤波器是对经典卡尔曼滤波器的一种扩展，旨在处理系统和观测模型中的非线性关系。它通过对非线性模型进行泰勒展开，将非线性问题线性化，从而应用线性卡尔曼滤波器。

主要步骤：

线性化模型：
对非线性状态转移方程和观测方程进行一阶泰勒展开，得到其在当前估计点处的线性化形式。
假设非线性状态方程和观测方程分别为： xk=f(xk−1)+wkx_k = f(x_{k-1}) + w_k zk=h(xk)+vkz_k = h(x_k) + v_k 其中 f(x) 是状态转移方程，h(x) 是观测方程，w_k 和 v_k 分别是过程噪声和观测噪声。
计算雅可比矩阵：
对状态转移方程 f(x) 和观测方程 h(x) 分别计算关于状态 x 的雅可比矩阵： Fk=∂f(xk−1)∂xk−1F_k = \frac{\partial f(x_{k-1})}{\partial x_{k-1}} Hk=∂h(xk)∂xkH_k = \frac{\partial h(x_k)}{\partial x_k}
雅可比矩阵用于将非线性问题线性化。
预测阶段：
使用线性化后的状态转移方程进行预测： x^k−=f(x^k−1)\hat{x}^-k = f(\hat{x}{k-1}) Pk−=Fk−1Pk−1Fk−1T+QP^-k = F{k-1} P_{k-1} F_{k-1}^T + Q 其中，P^-_k 是预测的协方差矩阵。
更新阶段：
使用观测数据进行更新，更新状态估计： yk=zk−h(x^k−)y_k = z_k - h(\hat{x}^-_k) Sk=HkPk−HkT+RS_k = H_k P^-_k H_k^T + R Kk=Pk−HkTSk−1K_k = P^-_k H_k^T S_k^{-1} x^k=x^k−+Kkyk\hat{x}_k = \hat{x}^-_k + K_k y_k Pk=(I−KkHk)Pk−P_k = (I - K_k H_k) P^-_k
其中，S_k 是卡尔曼增益所需的测量协方差矩阵。

优点与局限：

优点：
相对简单，能够处理常见的非线性问题。
适用于多种类型的非线性系统，尤其是在系统的非线性不太强烈时。
局限：
由于是线性化处理，若系统非线性程度较强，估计结果可能较差。
线性化过程中可能会丢失一些重要的非线性信息。

2. 无迹卡尔曼滤波器（UKF）

无迹卡尔曼滤波器（Unscented Kalman Filter）是一种改进的卡尔曼滤波方法，它通过引入无迹变换来处理非线性问题，而不需要像 EKF 那样对模型进行线性化。

主要思想：

UKF 的基本思想是通过选择一组“精确”的样本点（称为“sigma 点”），来表示状态分布，并通过这些样本点的传播来估计非线性系统的状态和协方差。

生成 sigma 点：
从当前状态估计 x^\hat{x} 和协方差矩阵 PP 生成一组 sigma 点： χk(i)=x^k+((n+λ)Pk)(i),i=1,2,…,2n\chi_k^{(i)} = \hat{x}_k + (\sqrt{(n + \lambda)} P_k)^{(i)}, \quad i = 1, 2, \dots, 2n 其中，λ\lambda 是一个调节参数，nn 是状态向量的维度。
传播 sigma 点：
使用非线性状态转移方程 f(x)f(x) 和观测方程 h(x)h(x) 将每个 sigma 点传播到下一个时刻，得到预测的 sigma 点： χk(i)=f(χk−1(i))\chi_k^{(i)} = f(\chi_{k-1}^{(i)}) 然后根据这些 sigma 点来估计状态和协方差。
计算预测状态和协方差：
根据 sigma 点的加权平均来估计预测状态： x^k−=∑i=12nWi(m)χk(i)\hat{x}^-k = \sum{i=1}^{2n} W_i^{(m)} \chi_k^{(i)}
计算协方差矩阵： Pk−=∑i=12nWi(c)(χk(i)−x^k−)(χk(i)−x^k−)T+QP^-k = \sum{i=1}^{2n} W_i^{(c)} (\chi_k^{(i)} - \hat{x}^-_k)(\chi_k^{(i)} - \hat{x}^-_k)^T + Q
更新阶段：
类似于 EKF，使用观测值来更新状态估计，但在这里使用传播后的 sigma 点和观测模型来计算更新后的状态和协方差。

优点与局限：

优点：
通过无迹变换避免了线性化的误差，能够更准确地处理强非线性系统。
在高维非线性系统中，性能通常优于 EKF。
局限：
比 EKF 更复杂，计算量较大。
对于非常强的非线性问题，可能仍然无法获得完全准确的估计。

3. 如何选择

EKF：适用于非线性程度较弱或中等的系统，计算较简单。
UKF：适用于系统的非线性较强，或系统中状态空间的维度较高，能够提供更准确的估计，但计算复杂度较高。

示例：扩展卡尔曼滤波器（EKF）的 Python 实现

假设我们要跟踪一个具有非线性动态模型的物体。物体的运动状态由以下非线性状态方程给出：

xk=f(xk−1)+wkx_k = f(x_{k-1}) + w_kf(xk−1)=[xk−1+v⋅Δtyk−1+v⋅Δt]f(x_{k-1}) = \begin{bmatrix} x_{k-1} + v \cdot \Delta t \ y_{k-1} + v \cdot \Delta t \end{bmatrix}

假设观测模型是线性的，观测值仅包括物体的 x 和 y 坐标。

import numpy as np
import matplotlib.pyplot as plt

# 定义状态转移方程（非线性）
def state_transition(x, v, dt):
    return np.array([x[0] + v * dt, x[1] + v * dt])

# 定义观测方程（线性）
def observation_model(x):
    return np.array([x[0], x[1]])

# 初始化
dt = 1.0
v = 1.0  # 速度假设为常量
x_true = np.array([0, 0])  # 初始真实位置
P = np.eye(2)  # 初始协方差
R = np.eye(2)  # 观测噪声协方差
Q = np.eye(2) * 0.01  # 过程噪声协方差

# 观测值
measurements = [np.array([1, 1]), np.array([2, 2]), np.array([3, 3])]

# 扩展卡尔曼滤波器
x_est = np.array([0, 0])  # 初始估计
estimated_positions = []

for z in measurements:
    # 预测阶段
    x_pred = state_transition(x_est, v, dt)
    F = np.array([[1, 0], [0, 1]])  # 状态转移矩阵（在简单模型中是单位矩阵）
    P_pred = F @ P @ F.T + Q

    # 更新阶段
    H = np.eye(2)  # 观测矩阵（线性）
    y = z - observation_model(x_pred)  # 测量残差
    S = H @ P_pred @ H.T + R  # 卡尔曼增益的分母
    K = P_pred @ H.T @ np.linalg.inv(S)  # 卡尔曼增益
    x_est = x_pred + K @ y  # 更新估计
    P = (np.eye(2) - K @ H) @ P_pred  # 更新协方差

    estimated_positions.append(x_est)

# 绘图
estimated_positions = np.array(estimated_positions) 
plt.plot(estimated_positions[:, 0], estimated_positions[:, 1], label="Estimated Position") 
plt.scatter([1, 2, 3], [1, 2, 3], color='r', label="Measurements") 
plt.legend() 
plt.xlabel('x') 
plt.ylabel('y') 
plt.show()