第4章：线性卡尔曼滤波算法实现

学习目标

• 掌握标准卡尔曼滤波算法的五个核心方程
• 理解协方差矩阵的传播和更新
• 学会算法的数值实现和编程技巧

五个核心方程

卡尔曼滤波算法包含两个主要阶段，共五个核心方程：

预测阶段（时间更新）

方程1 - 状态预测：

$$\hat{x}_{k|k-1} = F_k \hat{x}_{k-1|k-1} + B_k u_k$$

方程2 - 协方差预测：

$$P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k$$

更新阶段（测量更新）

方程3 - 卡尔曼增益：

$$K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}$$

方程4 - 状态更新：

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

方程5 - 协方差更新：

$$P_{k|k} = (I - K_k H_k) P_{k|k-1}$$

算法流程图

完整算法实现

基本卡尔曼滤波类

import numpy as np
from typing import Optional, Tuple
import matplotlib.pyplot as plt

class KalmanFilter:
    """
    线性卡尔曼滤波器实现
    """

    def __init__(self, dim_x: int, dim_z: int, dim_u: int = 0):
        """
        初始化卡尔曼滤波器

        参数:
        dim_x: 状态向量维度
        dim_z: 观测向量维度
        dim_u: 控制输入维度
        """
        self.dim_x = dim_x
        self.dim_z = dim_z
        self.dim_u = dim_u

        # 状态向量和协方差矩阵
        self.x = np.zeros((dim_x, 1))  # 状态估计
        self.P = np.eye(dim_x)         # 协方差矩阵

        # 系统矩阵
        self.F = np.eye(dim_x)         # 状态转移矩阵
        self.H = np.zeros((dim_z, dim_x))  # 观测矩阵
        self.Q = np.eye(dim_x)         # 过程噪声协方差
        self.R = np.eye(dim_z)         # 观测噪声协方差

        if dim_u > 0:
            self.B = np.zeros((dim_x, dim_u))  # 控制输入矩阵
        else:
            self.B = None

        # 辅助变量
        self.x_prior = self.x.copy()   # 先验状态估计
        self.P_prior = self.P.copy()   # 先验协方差
        self.K = np.zeros((dim_x, dim_z))  # 卡尔曼增益
        self.y = np.zeros((dim_z, 1))  # 残差（创新）
        self.S = np.zeros((dim_z, dim_z))  # 创新协方差

        # 似然和日志似然
        self.log_likelihood = 0.0
        self.likelihood = 0.0

    def predict(self, u: Optional[np.ndarray] = None,
                B: Optional[np.ndarray] = None,
                F: Optional[np.ndarray] = None,
                Q: Optional[np.ndarray] = None):
        """
        预测步骤（时间更新）

        参数:
        u: 控制输入
        B: 控制输入矩阵（可选，覆盖self.B）
        F: 状态转移矩阵（可选，覆盖self.F）
        Q: 过程噪声协方差（可选，覆盖self.Q）
        """

        if B is not None:
            self.B = B
        if F is not None:
            self.F = F
        if Q is not None:
            self.Q = Q

        # 方程1：状态预测
        if self.B is not None and u is not None:
            self.x = self.F @ self.x + self.B @ u
        else:
            self.x = self.F @ self.x

        # 方程2：协方差预测
        self.P = self.F @ self.P @ self.F.T + self.Q

        # 保存先验估计
        self.x_prior = self.x.copy()
        self.P_prior = self.P.copy()

    def update(self, z: np.ndarray,
               R: Optional[np.ndarray] = None,
               H: Optional[np.ndarray] = None):
        """
        更新步骤（测量更新）

        参数:
        z: 观测向量
        R: 观测噪声协方差（可选，覆盖self.R）
        H: 观测矩阵（可选，覆盖self.H）
        """

        if R is not None:
            self.R = R
        if H is not None:
            self.H = H

        # 确保z是列向量
        if z.ndim == 1:
            z = z.reshape((-1, 1))

        # 计算残差（创新）
        self.y = z - self.H @ self.x

        # 计算创新协方差
        self.S = self.H @ self.P @ self.H.T + self.R

        # 方程3：计算卡尔曼增益
        self.K = self.P @ self.H.T @ np.linalg.inv(self.S)

        # 方程4：状态更新
        self.x = self.x + self.K @ self.y

        # 方程5：协方差更新（Joseph形式，数值稳定）
        I_KH = np.eye(self.dim_x) - self.K @ self.H
        self.P = I_KH @ self.P @ I_KH.T + self.K @ self.R @ self.K.T

        # 计算似然
        self._update_likelihood()

    def _update_likelihood(self):
        """计算当前观测的似然"""
        det_S = np.linalg.det(self.S)
        if det_S <= 0:
            self.log_likelihood = -np.inf
            self.likelihood = 0.0
        else:
            self.log_likelihood = (-0.5 *
                                 (self.y.T @ np.linalg.inv(self.S) @ self.y +
                                  np.log(det_S) +
                                  self.dim_z * np.log(2 * np.pi))).item()
            self.likelihood = np.exp(self.log_likelihood)

协方差更新的数值稳定形式

卡尔曼滤波在实际应用中可能面临数值不稳定问题，特别是协方差矩阵的更新：

def joseph_form_update(self):
    """
    Joseph形式的协方差更新，保证数值稳定性和正定性

    标准形式: P = (I - KH)P
    Joseph形式: P = (I - KH)P(I - KH)ᵀ + KRKᵀ
    """
    I_KH = np.eye(self.dim_x) - self.K @ self.H
    self.P = I_KH @ self.P @ I_KH.T + self.K @ self.R @ self.K.T

def square_root_update(self):
    """
    平方根形式的更新，保证协方差矩阵的正定性
    """
    # 使用Cholesky分解
    try:
        # P = LLᵀ形式
        L = np.linalg.cholesky(self.P)
        # 更新平方根因子
        # 这里省略具体的平方根更新算法
        pass
    except np.linalg.LinAlgError:
        # 如果Cholesky分解失败，使用regularization
        self.P += 1e-9 * np.eye(self.dim_x)

一维示例：跟踪匀速运动物体

def track_1d_constant_velocity():
    """
    一维匀速运动跟踪示例
    """

    # 系统参数
    dt = 1.0  # 时间步长

    # 创建卡尔曼滤波器：状态 [位置, 速度]
    kf = KalmanFilter(dim_x=2, dim_z=1)

    # 状态转移矩阵（匀速运动模型）
    kf.F = np.array([[1, dt],
                     [0, 1]])

    # 观测矩阵（只观测位置）
    kf.H = np.array([[1, 0]])

    # 过程噪声协方差
    q = 0.1  # 过程噪声强度
    kf.Q = q * np.array([[dt**4/4, dt**3/2],
                         [dt**3/2, dt**2]])

    # 观测噪声协方差
    kf.R = np.array([[1.0]])  # 观测噪声方差

    # 初始状态和协方差
    kf.x = np.array([[0], [1]])  # 初始位置0，速度1
    kf.P = np.array([[1000, 0],
                     [0, 1000]])  # 大的初始不确定性

    # 模拟真实运动
    true_positions = []
    observations = []
    estimates = []

    true_pos = 0
    true_vel = 1

    for k in range(50):
        # 真实运动
        true_pos += true_vel * dt
        true_positions.append(true_pos)

        # 生成带噪声的观测
        obs = true_pos + np.random.normal(0, 1)
        observations.append(obs)

        # 卡尔曼滤波
        kf.predict()
        kf.update(np.array([obs]))

        estimates.append(kf.x[0, 0])

    # 绘制结果
    plt.figure(figsize=(12, 8))

    plt.subplot(2, 1, 1)
    plt.plot(true_positions, 'g-', label='真实位置', linewidth=2)
    plt.plot(observations, 'r.', label='观测', alpha=0.7)
    plt.plot(estimates, 'b-', label='卡尔曼估计', linewidth=2)
    plt.legend()
    plt.title('一维匀速运动跟踪')
    plt.ylabel('位置')

    plt.subplot(2, 1, 2)
    errors = np.array(estimates) - np.array(true_positions)
    plt.plot(errors, 'r-', label='估计误差')
    plt.axhline(y=0, color='k', linestyle='--', alpha=0.5)
    plt.legend()
    plt.xlabel('时间步')
    plt.ylabel('误差')

    plt.tight_layout()
    plt.show()

    return true_positions, observations, estimates

# 运行示例
# track_1d_constant_velocity()

二维示例：跟踪匀加速运动

def track_2d_constant_acceleration():
    """
    二维匀加速运动跟踪示例
    """

    dt = 0.1  # 时间步长

    # 状态向量：[x, vx, ax, y, vy, ay]
    kf = KalmanFilter(dim_x=6, dim_z=2)

    # 状态转移矩阵
    kf.F = np.array([
        [1, dt, dt**2/2, 0, 0, 0],
        [0, 1, dt, 0, 0, 0],
        [0, 0, 1, 0, 0, 0],
        [0, 0, 0, 1, dt, dt**2/2],
        [0, 0, 0, 0, 1, dt],
        [0, 0, 0, 0, 0, 1]
    ])

    # 观测矩阵（观测位置）
    kf.H = np.array([
        [1, 0, 0, 0, 0, 0],
        [0, 0, 0, 1, 0, 0]
    ])

    # 过程噪声协方差（只在加速度上添加噪声）
    q = 0.1
    kf.Q = np.zeros((6, 6))
    kf.Q[2, 2] = q  # x方向加速度噪声
    kf.Q[5, 5] = q  # y方向加速度噪声

    # 观测噪声协方差
    kf.R = np.array([[0.1, 0],
                     [0, 0.1]])

    # 初始化
    kf.x = np.array([[0], [1], [0.1], [0], [1], [0.1]])  # 初始状态
    kf.P = 100 * np.eye(6)  # 初始协方差

    # 模拟和滤波
    positions = []
    estimates = []

    for k in range(100):
        # 预测和更新
        kf.predict()

        # 模拟观测（这里假设我们有真实观测）
        true_x = k * dt + 0.5 * 0.1 * (k * dt)**2
        true_y = k * dt + 0.5 * 0.1 * (k * dt)**2
        obs = np.array([true_x + np.random.normal(0, 0.1),
                       true_y + np.random.normal(0, 0.1)])

        kf.update(obs)

        positions.append([true_x, true_y])
        estimates.append([kf.x[0, 0], kf.x[3, 0]])

    # 绘制轨迹
    positions = np.array(positions)
    estimates = np.array(estimates)

    plt.figure(figsize=(10, 8))
    plt.plot(positions[:, 0], positions[:, 1], 'g-',
             label='真实轨迹', linewidth=2)
    plt.plot(estimates[:, 0], estimates[:, 1], 'b-',
             label='卡尔曼估计', linewidth=2)
    plt.legend()
    plt.title('二维匀加速运动跟踪')
    plt.xlabel('X位置')
    plt.ylabel('Y位置')
    plt.grid(True)
    plt.axis('equal')
    plt.show()

# 运行示例
# track_2d_constant_acceleration()

算法性能分析

计算复杂度

卡尔曼滤波的时间复杂度分析：

操作	复杂度	说明
状态预测	$O(n^2)$	矩阵乘法 $F \cdot x$
协方差预测	$O(n^3)$	矩阵乘法 $F \cdot P \cdot F^T$
卡尔曼增益	$O(m^3 + nm^2)$	矩阵求逆和乘法
状态更新	$O(nm)$	矩阵乘法
协方差更新	$O(n^2m + n^3)$	Joseph形式

总复杂度：$O(n^3 + m^3 + n^2m)$，其中$n$是状态维度，$m$是观测维度。

数值稳定性技巧

class StableKalmanFilter(KalmanFilter):
    """
    数值稳定的卡尔曼滤波实现
    """

    def __init__(self, *args, **kwargs):
        super().__init__(*args, **kwargs)
        self.regularization = 1e-12

    def _regularize_covariance(self):
        """正则化协方差矩阵，确保正定性"""
        eigenvals = np.linalg.eigvals(self.P)
        if np.any(eigenvals <= 0):
            self.P += self.regularization * np.eye(self.dim_x)

    def _check_condition_number(self):
        """检查条件数，如果过大则进行处理"""
        cond_P = np.linalg.cond(self.P)
        cond_S = np.linalg.cond(self.S)

        if cond_P > 1e12 or cond_S > 1e12:
            print(f"警告：条件数过大 P:{cond_P:.2e}, S:{cond_S:.2e}")
            self._regularize_covariance()

    def update(self, z, R=None, H=None):
        """数值稳定的更新"""
        super().update(z, R, H)
        self._regularize_covariance()
        self._check_condition_number()

参数调优指导

噪声协方差矩阵设置

参数调优原则

• 过程噪声Q：反映模型的不确定性，Q太小会导致滤波器过度自信，Q太大会导致估计不稳定
• 观测噪声R：反映测量的不确定性，应基于传感器规格设置
• 初始协方差P₀：反映初始估计的不确定性，通常设置较大值

def parameter_tuning_example():
    """
    参数调优示例
    """

    # 不同Q值的比较
    q_values = [0.01, 0.1, 1.0, 10.0]

    for q in q_values:
        kf = KalmanFilter(dim_x=2, dim_z=1)

        # 设置系统矩阵
        dt = 1.0
        kf.F = np.array([[1, dt], [0, 1]])
        kf.H = np.array([[1, 0]])
        kf.Q = q * np.array([[dt**4/4, dt**3/2],
                             [dt**3/2, dt**2]])
        kf.R = np.array([[1.0]])

        # 运行滤波并分析性能
        # ... 具体实现略

        print(f"Q = {q}: 性能指标...")

# 运行参数调优
# parameter_tuning_example()

常见问题和解决方案

1. 滤波发散

症状：估计误差随时间增长
原因：

• Q矩阵设置过小
• 模型不匹配
• 数值不稳定

解决方案：

def divergence_detection(kf, threshold=1e6):
    """检测滤波发散"""
    trace_P = np.trace(kf.P)
    if trace_P > threshold:
        print(f"警告：协方差迹过大 {trace_P:.2e}")
        return True
    return False

2. 协方差矩阵非正定

解决方案：

def ensure_positive_definite(P, min_eigenval=1e-12):
    """确保矩阵正定"""
    eigenvals, eigenvecs = np.linalg.eigh(P)
    eigenvals = np.maximum(eigenvals, min_eigenval)
    return eigenvecs @ np.diag(eigenvals) @ eigenvecs.T

下一章预告

下一章我们将学习扩展卡尔曼滤波（EKF），了解如何处理非线性系统的状态估计问题。