卡尔曼滤波与扩展方法

概述

卡尔曼滤波（Kalman Filter）是贝叶斯滤波在线性高斯系统下的精确解，用于从噪声观测中估计动态系统的隐藏状态[^1][^2]。它广泛应用于目标跟踪、导航、金融时序和机器人定位等领域。

核心思想：利用系统动态模型和观测模型，在预测-更新（Predict-Update）的迭代框架下，逐步更新状态估计。

---

1. 状态空间模型基础

1.1 连续与离散时间模型

连续时间状态空间模型：

$$\frac{d\mathbf{x}(t)}{dt}=F(t)\mathbf{x}(t)+\mathbf{w}(t),\mathbf{w}(t)\sim \mathcal{N}(0,Q(t))$$ $$\mathbf{y}(t)=H(t)\mathbf{x}(t)+\mathbf{v}(t),\mathbf{v}(t)\sim \mathcal{N}(0,R(t))$$

离散时间状态空间模型（卡尔曼滤波处理的对象）：

$${\mathbf{x}}_{t+1}=F_t{\mathbf{x}}_t+{\mathbf{w}}_t,{\mathbf{w}}_t\sim \mathcal{N}(0,Q_t)\text{\hspace{1em}(1)}$$ $${\mathbf{y}}_t=H_t{\mathbf{x}}_t+{\mathbf{v}}_t,{\mathbf{v}}_t\sim \mathcal{N}(0,R_t)\text{\hspace{1em}(2)}$$

其中：

• ${\mathbf{x}}_t\in {\mathbb{R}}^{n_x}$：状态向量
• ${\mathbf{y}}_t\in {\mathbb{R}}^{n_y}$：观测向量
• $F_t$：状态转移矩阵
• $H_t$：观测矩阵
• $Q_t$：过程噪声协方差
• $R_t$：观测噪声协方差

1.2 线性高斯假设

卡尔曼滤波的精确性来源于以下假设：

1. 线性动态：状态转移是线性的
2. 高斯噪声：过程噪声和观测噪声均为高斯分布
3. 初始先验：初始状态是高斯分布

在这些假设下，后验分布 $P({\mathbf{x}}_t|{\mathbf{y}}_{1:t})$ 也是高斯分布，可以用均值和协方差完全描述。

---

2. 卡尔曼滤波算法

2.1 预测与更新

预测步骤（Prediction）：

$${\hat{\mathbf{x}}}_{t|t-1}=F_t{\hat{\mathbf{x}}}_{t-1|t-1}$$ $$P_{t|t-1}=F_t{P}_{t-1|t-1}{F}_t^T+Q_t$$

更新步骤（Update）：

$$S_t=H_t{P}_{t|t-1}{H}_t^T+R_t\text{（预测观测协方差）}$$ $$K_t=P_{t|t-1}{H}_t^T{S}_t^{-1}\text{（卡尔曼增益）}$$ $${\hat{\mathbf{x}}}_{t|t}={\hat{\mathbf{x}}}_{t|t-1}+K_t({\mathbf{y}}_t-H_t{\hat{\mathbf{x}}}_{t|t-1})$$ $$P_{t|t}=(I-K_t{H}_t)P_{t|t-1}$$

2.2 卡尔曼增益的几何意义

卡尔曼增益 $K_t$ 平衡了预测和观测的相对可靠性：

$$K_t=\underset{\text{状态-观测相关性}}{\underbrace{P_{t|t-1}{H}_t^T}}\cdot \underset{\text{观测不确定性逆}}{\underbrace{S_t^{-1}}}$$

• 当 $R_t\to 0$（观测非常可靠）：$K_t\to H_t^{-1}$，信任观测
• 当 $P_{t|t-1}\to 0$（预测非常确定）：$K_t\to 0$，信任预测

2.3 Python实现

import numpy as np
from dataclasses import dataclass
from typing import Tuple, Optional
 
@dataclass
class KalmanFilter:
    """卡尔曼滤波实现"""
    
    F: np.ndarray  # 状态转移矩阵
    H: np.ndarray  # 观测矩阵
    Q: np.ndarray  # 过程噪声协方差
    R: np.ndarray  # 观测噪声协方差
    x: np.ndarray  # 初始状态估计
    P: np.ndarray  # 初始协方差估计
    
    def predict(self) -> Tuple[np.ndarray, np.ndarray]:
        """预测步骤"""
        self.x = self.F @ self.x
        self.P = self.F @ self.P @ self.F.T + self.Q
        return self.x, self.P
    
    def update(self, z: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
        """更新步骤"""
        # 预测观测
        y_pred = self.H @ self.x
        
        # 残差
        innovation = z - y_pred
        
        # 残差协方差
        S = self.H @ self.P @ self.H.T + self.R
        
        # 卡尔曼增益
        K = self.P @ self.H.T @ np.linalg.inv(S)
        
        # 更新状态和协方差
        self.x = self.x + K @ innovation
        self.P = (np.eye(len(self.x)) - K @ self.H) @ self.P
        
        return self.x, self.P
    
    def filter(self, observations: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
        """
        对观测序列运行卡尔曼滤波
        
        参数:
            observations: 观测序列 [T, n_y]
        返回:
            states: 滤波后的状态估计 [T, n_x]
            covariances: 对应的协方差矩阵 [T, n_x, n_x]
        """
        T = len(observations)
        n_x = len(self.x)
        
        states = np.zeros((T, n_x))
        covariances = np.zeros((T, n_x, n_x))
        
        for t in range(T):
            self.predict()
            self.update(observations[t])
            states[t] = self.x
            covariances[t] = self.P
        
        return states, covariances

---

3. 扩展卡尔曼滤波（EKF）

3.1 非线性系统的挑战

当系统动态或观测模型为非线性时：

$${\mathbf{x}}_{t+1}=f({\mathbf{x}}_t,{\mathbf{w}}_t)$$ $${\mathbf{y}}_t=h({\mathbf{x}}_t,{\mathbf{v}}_t)$$

精确的贝叶斯后验不再是高斯分布。EKF通过一阶泰勒展开线性化非线性函数。

3.2 EKF算法

线性化（在当前估计处展开）：

$$f({\mathbf{x}}_t)\approx f({\hat{\mathbf{x}}}_{t|t-1})+F_t({\mathbf{x}}_t-{\hat{\mathbf{x}}}_{t|t-1})$$ $$h({\mathbf{x}}_t)\approx h({\hat{\mathbf{x}}}_{t|t-1})+H_t({\mathbf{x}}_t-{\hat{\mathbf{x}}}_{t|t-1})$$

其中雅可比矩阵为：

$$F_t=\frac{\partial f}{\partial \mathbf{x}}{|}_{\mathbf{x}={\hat{\mathbf{x}}}_{t|t-1}},H_t=\frac{\partial h}{\partial \mathbf{x}}{|}_{\mathbf{x}={\hat{\mathbf{x}}}_{t|t-1}}$$

EKF步骤：

class ExtendedKalmanFilter:
    """扩展卡尔曼滤波"""
    
    def __init__(self, f, h, F_jac, H_jac, Q, R, x0, P0):
        self.f = f          # 非线性状态转移函数
        self.h = h          # 非线性观测函数
        self.F_jac = F_jac  # f的雅可比矩阵
        self.H_jac = H_jac  # h的雅可比矩阵
        self.Q = Q
        self.R = R
        self.x = x0
        self.P = P0
    
    def predict(self):
        """预测步骤"""
        self.x = self.f(self.x)
        F = self.F_jac(self.x)
        self.P = F @ self.P @ F.T + self.Q
        return self.x, self.P
    
    def update(self, z):
        """更新步骤"""
        H = self.H_jac(self.x)
        y_pred = self.h(self.x)
        
        innovation = z - y_pred
        S = H @ self.P @ H.T + self.R
        K = self.P @ H.T @ np.linalg.inv(S)
        
        self.x = self.x + K @ innovation
        self.P = (np.eye(len(self.x)) - K @ H) @ self.P
        
        return self.x, self.P

3.3 EKF的局限性

问题	描述	影响
线性化误差	一阶近似在强非线性处不准确	估计偏差
协方差退化	非线性区域协方差可能低估	过信估计
雅可比计算	复杂系统雅可比矩阵难以推导	实现困难

---

4. 无迹卡尔曼滤波（UKF）

4.1 UT变换（Unscented Transform）

UKF避免显式计算雅可比矩阵，而是通过确定性采样捕捉分布的统计特性[^3]。

对于 $n$ 维状态，选取 $2n+1$ 个Sigma点：

$${\mathcal{X}}_0=\mathbf{x}$$ $${\mathcal{X}}_i=\mathbf{x}+{\left(\sqrt{(n+\lambda )P}\right)}_i,i=1,...,n$$ $${\mathcal{X}}_i=\mathbf{x}-{\left(\sqrt{(n+\lambda )P}\right)}_{i-n},i=n+1,...,2n$$

权重计算：

$$W_0^{(m)}=\frac{\lambda }{n+\lambda },W_0^{(c)}=\frac{\lambda }{n+\lambda }+(1-{\alpha }^2+\beta )$$ $$W_i^{(m)}=W_i^{(c)}=\frac{1}{2(n+\lambda )},i=1,...,2n$$

4.2 UKF算法

class UnscentedKalmanFilter:
    """无迹卡尔曼滤波"""
    
    def __init__(self, f, h, Q, R, x0, P0, alpha=1e-3, beta=2, kappa=0):
        self.f = f
        self.h = h
        self.Q = Q
        self.R = R
        self.x = x0
        self.P = P0
        self.n = len(x0)
        
        # UKF参数
        self.lambda_ = alpha**2 * (self.n + kappa) - self.n
        self.Wm = np.zeros(2 * self.n + 1)
        self.Wc = np.zeros(2 * self.n + 1)
        self._compute_weights()
    
    def _compute_weights(self):
        """计算UT权重"""
        n = self.n
        lam = self.lambda_
        self.Wm[0] = lam / (n + lam)
        self.Wc[0] = lam / (n + lam) + (1 - alpha**2 + beta)
        for i in range(1, 2 * n + 1):
            self.Wm[i] = self.Wc[i] = 1 / (2 * (n + lam))
    
    def _sigma_points(self):
        """生成Sigma点"""
        n = self.n
        lam = self.lambda_
        
        # 矩阵平方根
        sqrt_P = np.linalg.cholesky((n + lam) * self.P)
        
        sigma = np.zeros((2 * n + 1, n))
        sigma[0] = self.x
        
        for i in range(n):
            sigma[i + 1] = self.x + sqrt_P[:, i]
            sigma[n + i + 1] = self.x - sqrt_P[:, i]
        
        return sigma
    
    def predict(self):
        """预测步骤"""
        sigma = self._sigma_points()
        n = self.n
        
        # 通过非线性函数传播Sigma点
        sigma_pred = np.array([self.f(s) for s in sigma])
        
        # 加权计算预测均值和协方差
        x_pred = np.sum(self.Wm[:, np.newaxis] * sigma_pred, axis=0)
        P_pred = self.Q.copy()
        for i in range(2 * n + 1):
            diff = sigma_pred[i] - x_pred
            P_pred += self.Wc[i] * np.outer(diff, diff)
        
        self.x = x_pred
        self.P = P_pred
        
        return sigma_pred, x_pred, P_pred
    
    def update(self, z):
        """更新步骤"""
        sigma, x_pred, P_pred = self.predict()
        n = self.n
        
        # 传播到观测空间
        sigma_obs = np.array([self.h(s) for s in sigma])
        
        # 预测观测
        z_pred = np.sum(self.Wm[:, np.newaxis] * sigma_obs, axis=0)
        
        # 观测协方差和互协方差
        S = self.R.copy()
        Pxz = np.zeros((n, len(z_pred)))
        
        for i in range(2 * n + 1):
            diff_z = sigma_obs[i] - z_pred
            diff_x = sigma[i] - x_pred
            S += self.Wc[i] * np.outer(diff_z, diff_z)
            Pxz += self.Wc[i] * np.outer(diff_x, diff_z)
        
        # 卡尔曼增益和更新
        K = Pxz @ np.linalg.inv(S)
        innovation = z - z_pred
        
        self.x = x_pred + K @ innovation
        self.P = P_pred - K @ S @ K.T
        
        return self.x, self.P

4.3 UKF vs EKF

特性	EKF	UKF
近似阶数	一阶	至少二阶
雅可比计算	需要	不需要
计算复杂度	$O(n^2)$	$O((2n+1)n^2)$
精度	弱非线性	强非线性
收敛性	可能发散	更稳定

---

5. 粒子滤波（Sequential Importance Sampling）

5.1 基本思想

当状态空间连续且非线性很强时，使用蒙特卡洛采样近似后验分布：

$$P({\mathbf{x}}_t|{\mathbf{y}}_{1:t})\approx \sum _{i=1}^N{w}_t^{(i)}\delta ({\mathbf{x}}_t-{\mathbf{x}}_t^{(i)})$$

其中 $w_t^{(i)}$ 是归一化重要性权重。

5.2 重要性采样基础

目标分布 $p(\mathbf{x})$ 难以采样，从提议分布 $q(\mathbf{x})$ 采样：

$${\mathbb{E}}_p[f(\mathbf{x})]=\int f(\mathbf{x})p(\mathbf{x})d\mathbf{x}=\int f(\mathbf{x})\frac{p(\mathbf{x})}{q(\mathbf{x})}q(\mathbf{x})d\mathbf{x}$$

重要性权重：

$$w({\mathbf{x}}^{(i)})=\frac{p({\mathbf{x}}^{(i)})}{q({\mathbf{x}}^{(i)})}$$

5.3 序贯重要性采样（SIS）

递归更新权重：

$$w_t^{(i)}\propto w_{t-1}^{(i)}\frac{p({\mathbf{y}}_t|{\mathbf{x}}_t^{(i)})p({\mathbf{x}}_t^{(i)}|{\mathbf{x}}_{t-1}^{(i)})}{q({\mathbf{x}}_t^{(i)}|{\mathbf{x}}_{t-1}^{(i)},{\mathbf{y}}_t)}$$

5.4 粒子滤波完整实现

class ParticleFilter:
    """粒子滤波（序贯重要性采样 + 重采样）"""
    
    def __init__(self, prior_sampler, transition, likelihood, n_particles):
        """
        参数:
            prior_sampler: 先验采样函数 () -> x
            transition: 转移函数 p(x'|x)
            likelihood: 似然函数 p(y|x)
            n_particles: 粒子数量
        """
        self.prior_sampler = prior_sampler
        self.transition = transition
        self.likelihood = likelihood
        self.n_particles = n_particles
        self.particles = None
        self.weights = None
        self._init_particles()
    
    def _init_particles(self):
        """初始化粒子"""
        self.particles = np.array([self.prior_sampler() for _ in range(self.n_particles)])
        self.weights = np.ones(self.n_particles) / self.n_particles
    
    def _resample(self):
        """系统重采样"""
        cumsum = np.cumsum(self.weights)
        cumsum[-1] = 1.0  # 避免浮点误差
        
        # 系统重采样
        step = 1.0 / self.n_particles
        start = np.random.uniform(0, step)
        indices = np.zeros(self.n_particles, dtype=int)
        
        j = 0
        for i in range(self.n_particles):
            target = start + i * step
            while cumsum[j] < target:
                j += 1
            indices[i] = j
        
        self.particles = self.particles[indices]
        self.weights = np.ones(self.n_particles) / self.n_particles
    
    def predict(self):
        """预测：采样新的粒子"""
        self.particles = np.array([
            self.transition(p) for p in self.particles
        ])
    
    def update(self, observation):
        """更新：计算权重"""
        likelihoods = np.array([
            self.likelihood(observation, p) for p in self.particles
        ])
        self.weights *= likelihoods
        self.weights /= np.sum(self.weights)
    
    def effective_sample_size(self):
        """有效样本量"""
        return 1.0 / np.sum(self.weights ** 2)
    
    def step(self, observation, resample=True, min_ess_ratio=0.5):
        """单步滤波"""
        self.predict()
        self.update(observation)
        
        # 重采样
        if resample:
            ess = self.effective_sample_size()
            if ess < self.n_particles * min_ess_ratio:
                self._resample()
        
        return self.particles, self.weights
    
    def estimate(self):
        """状态估计"""
        return np.sum(self.weights[:, np.newaxis] * self.particles, axis=0)

5.5 重采样方法对比

方法	复杂度	优点	缺点
Multinomial	$O(N)$	理论正确	方差较大
Residual	$O(N)$	减少方差	实现稍复杂
Stratified	$O(N)$	均匀覆盖	依赖随机性
Systematic	$O(1)$	最小方差	可能有偏差

def resample_systematic(weights):
    """系统重采样"""
    N = len(weights)
    u0 = np.random.uniform(0, 1.0 / N)
    
    cumsum = np.cumsum(weights)
    positions = u0 + np.arange(N) / N
    
    indices = np.zeros(N, dtype=int)
    j = 0
    for i in range(N):
        while cumsum[j] < positions[i]:
            j += 1
        indices[i] = j
    
    return indices

---

6. 高级滤波方法

6.1 Rao-Blackwellized粒子滤波

利用条件结构减少方差：

$$P({\mathbf{x}}_t|{\mathbf{y}}_{1:t})=P({\mathbf{x}}_t^{(1)}|{\mathbf{x}}_t^{(2)},{\mathbf{y}}_{1:t})P({\mathbf{x}}_t^{(2)}|{\mathbf{y}}_{1:t})$$

其中 ${\mathbf{x}}_t^{(1)}$ 解析积分，${\mathbf{x}}_t^{(2)}$ 用粒子滤波。

6.2 辅助粒子滤波

改进建议分布：

$$q({\mathbf{x}}_t^{(i)}|{\mathbf{x}}_{t-1}^{(i)},{\mathbf{y}}_t)\propto p({\mathbf{y}}_t|{\mathbf{x}}_t^{(i)})p({\mathbf{x}}_t^{(i)}|{\mathbf{x}}_{t-1}^{(i)})$$

6.3 正则化粒子滤波

用核密度估计平滑后验：

$$\hat{p}({\mathbf{x}}_t|{\mathbf{y}}_{1:t})=\sum _{i=1}^N{w}_t^{(i)}{K}_h({\mathbf{x}}_t-{\mathbf{x}}_t^{(i)})$$

---

7. 应用实例

7.1 一维目标跟踪

# 一维匀速运动模型
dt = 0.1
F = np.array([[1, dt], [0, 1]])
H = np.array([[1, 0]])
 
# 过程噪声和观测噪声
q = 0.01
Q = q * np.array([[dt**3/3, dt**2/2], [dt**2/2, dt]])
R = np.array([[0.1]])
 
# 初始化
kf = KalmanFilter(F, H, Q, R, x0=np.array([0, 1]), P0=np.eye(2))
 
# 运行滤波
observations = np.array([[z] for z in [1.2, 2.1, 2.8, 4.2, 5.0]])
states, _ = kf.filter(observations)

7.2 机器人定位

非线性运动模型：

$$\left(\begin{array}{c}{x}_{t+1}\\ y_{t+1}\\ {\theta }_{t+1}\end{array}\right)=\left(\begin{array}{c}{x}_t+\Delta s\cos {\theta }_t\\ y_t+\Delta s\sin {\theta }_t\\ {\theta }_t+\Delta \theta \end{array}\right)+{\mathbf{w}}_t$$

观测模型（距离传感器）：

z_t = \sqrt{(x_t - x_{landmark})^2 + (y_t - y_{landmark})^2} + v_t ``` --- ## 8. 总结与展望 ### 方法对比 | 方法 | 线性/非线性 | 高斯/非高斯 | 计算复杂度 | |------|------------|------------|------------| | KF | 线性 | 高斯 | $O(n^2)$ | | EKF | 弱非线性 | 高斯 | $O(n^2)$ | | UKF | 非线性 | 高斯 | $O((2n+1)n^2)$ | | PF | 任意 | 任意 | $O(N n)$ | ### 选择指南 - **线性高斯系统**：使用KF - **弱非线性**：使用EKF或UKF - **强非线性或非高斯**：使用PF - **混合系统**：考虑RBPF ### 未来方向 - **深度卡尔曼滤波**：神经网络学习系统动态 - **变分滤波**：处理非高斯非高斯 - **分布式滤波**：多传感器融合 --- ## 参考资料 [^1]: Kalman, R. E. (1960). "A New Approach to Linear Filtering and Prediction Problems." Journal of Basic Engineering. [^2]: Welch, G., & Bishop, G. (2006). "An Introduction to the Kalman Filter." UNC Chapel Hill TR 95-041. [^3]: Julier, S. J., & Uhlmann, J. K. (1997). "New Extension of the Kalman Filter to Nonlinear Systems." Signal Processing, Sensing, and Control. [^4]: Doucet, A., de Freitas, N., & Gordon, N. (2001). "Sequential Monte Carlo Methods in Practice." Springer. --- ## 相关主题 - [[dynamic-bayesian-networks]] — 动态贝叶斯网络 - [[hidden-markov-model]] — 隐马尔可夫模型 - [[mcmc-methods]] — 马尔可夫链蒙特卡洛方法 - [[markov-chain]] — 马尔可夫链基础 - [[bayesian-inference]] — 贝叶斯推断 - [[variational-inference]] — 变分推断