卡尔曼滤波
卡尔曼滤波
初步
适用系统
线性、高斯系统。系统变化是线性的，而噪声是高斯分布的。
- 传感器数据融合（IMU、GPS等）
- 目标跟踪（雷达、视觉）
- 导航系统（无人机、机器人）
- 信号处理
- 控制系统状态估计
基本思想
- 估计值、观测值
- 通过融合估计值和观测值（加权）得到输出结果
理论
2.1 系统方程
状态方程（预测）
$$ x_k = F_k x_{k-1} + B_k u_k + w_k $$
参数说明： - $x_k$：k时刻状态向量 - $F_k$：状态转移矩阵 - $B_k$：控制输入矩阵 - $u_k$：控制输入向量（控制输入向量 $u_k$ 是外部施加给系统的控制量，用于主动改变系统状态。） - $w_k$：过程噪声，$w_k \sim N(0, Q_k)$，$Q_k$为$u_k$的协方差
其中，我们需要调节的参数是$Q_k$
观测方程（测量）
$$ z_k = H_k x_k + v_k $$
参数说明： - $z_k$：k时刻观测向量 - $H_k$：观测矩阵 - $v_k$：观测噪声，$v_k \sim N(0, R_k)$, $R_k$为$v_k$的协方差
其中，我们需要调节的参数是$R_k$
算法流程
3.1 预测步骤（时间更新）
符号说明
- $\hat{x}_{k|k-1}$表示由k-1的输出状态得到的k预测状态
- $\hat{x}_{k-1|k-1}$表示k-1的输出状态
状态预测
$$ \hat{x}{k|k-1} = F_k \hat{x} + B_k u_k+w_k $$
协方差预测
对上一个公式取协方差，又概率论知识得到如下公式（粗暴的理解，$F_kF_k^T$是一个平方；而$u_k$是理想的控制输入，没有方差）： $$ P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k $$
3.2 更新步骤（测量更新）
卡尔曼增益计算
$$ K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1} $$
状态更新
$$ \hat{x}{k|k} = \hat{x}) $$} + K_k (z_k - H_k \hat{x}_{k|k-1
协方差更新
$$ P_{k|k} = (I - K_k H_k) P_{k|k-1} $$
一维卡尔曼滤波
4.1 问题描述
估计一个匀速运动物体的位置，测量值包含噪声。
4.2 状态定义
基本方程：$p_i=p_{i-1}+v_{i-1}\Delta t+\frac{1}{2}a{\Delta t}^2$ $$ x = \begin{bmatrix} p \ v \end{bmatrix} $$ - $p$：位置 - $v$：速度
4.3 参数设置
# 状态转移矩阵（匀速模型）
F = np.array([[1, dt],
[0, 1]])
# 控制输入矩阵
B = np.array([[0.5*dt**2],
[dt]])
# 观测矩阵（只观测位置）
H = np.array(1, 0)
# 过程噪声协方差
Q = np.array([[0.1, 0],
[0, 0.1]])
# 观测噪声协方差
R = np.array(1.0)
# 初始状态
x = np.array([[0], # 位置
[0]]) # 速度
# 初始协方差
P = np.array([[1000, 0],
[0, 1000]])
4.4 算法实现
def kalman_filter(x, P, z, F, H, Q, R):
"""
一维卡尔曼滤波实现
"""
# 预测步骤
x_pred = F @ x
P_pred = F @ P @ F.T + Q
# 更新步骤
y = z - H @ x_pred # 残差
S = H @ P_pred @ H.T + R # 残差协方差
K = P_pred @ H.T @ np.linalg.inv(S) # 卡尔曼增益
# 状态更新
x = x_pred + K @ y
P = (np.eye(2) - K @ H) @ P_pred
return x, P
扩展卡尔曼滤波（EKF）
5.1 非线性系统处理
当系统方程为非线性时： $$ x_k = f(x_{k-1}, u_k) + w_k $$ $$ z_k = h(x_k) + v_k $$
5.2 线性化方法
使用雅可比矩阵进行局部线性化：
状态转移雅可比
$$ F_k = \frac{\partial f}{\partial x} \bigg|{x=\hat{x} $$}
观测雅可比
$$ H_k = \frac{\partial h}{\partial x} \bigg|{x=\hat{x} $$}
5.3 EKF算法步骤
-
预测： $$ \hat{x}{k|k-1} = f(\hat{x}, u_k) $$ $$ P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k $$
-
更新： $$ K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1} $$ $$ \hat{x}{k|k} = \hat{x}} + K_k [z_k - h(\hat{x{k|k-1})] $$ $$ P $$} = (I - K_k H_k) P_{k|k-1
无迹卡尔曼滤波（UKF）
6.1 核心思想
使用无迹变换（Unscented Transform）处理非线性，避免雅可比矩阵计算。
6.2 Sigma点采样
对于n维状态向量，选择$2n+1$个Sigma点： $$ \mathcal{X}^{(0)} = \bar{x} $$ $$ \mathcal{X}^{(i)} = \bar{x} + (\sqrt{(n+\lambda)P})i, \quad i=1,...,n $$ $$ \mathcal{X}^{(i)} = \bar{x} - (\sqrt{(n+\lambda)P}), \quad i=n+1,...,2n $$
6.3 权重计算
$$ W_m^{(0)} = \frac{\lambda}{n+\lambda} $$ $$ W_c^{(0)} = \frac{\lambda}{n+\lambda} + (1-\alpha^2+\beta) $$ $$ W_m^{(i)} = W_c^{(i)} = \frac{1}{2(n+\lambda)}, \quad i=1,...,2n $$
注意事项
7.1 参数调优
过程噪声Q
- 过大：滤波器过于信任测量值，响应快但噪声大
- 过小：滤波器过于信任预测值，响应慢但平滑
观测噪声R
- 过大：滤波器过于信任预测值
- 过小：滤波器过于信任测量值
7.2 初始化策略
- 状态初始化：使用第一次测量值
- 协方差初始化：设置较大的初始不确定性
- 收敛判断：协方差矩阵对角线元素趋于稳定
7.3 数值稳定性
- 使用平方根卡尔曼滤波避免协方差矩阵不正定
- 定期检查协方差矩阵的对称性和正定性
常见问题与解决
10.1 发散问题
现象：估计误差不断增大 原因： 1. 模型不准确 2. 噪声统计特性变化 3. 数值计算误差累积
解决方案： 1. 增加过程噪声Q 2. 使用自适应卡尔曼滤波 3. 定期重置协方差矩阵
10.2 延迟问题
现象：估计值滞后于真实值 原因：滤波器过于平滑 解决方案：减小过程噪声Q，增加对测量的信任
10.3 计算复杂度
优化方法： 1. 使用稀疏矩阵运算 2. 并行计算卡尔曼增益 3. 固定增益近似（α-β滤波器）