卡尔曼滤波的统计性能

### 卡尔曼滤波器的统计性能特性

卡尔曼滤波是一种用于估计对象底层运动动态的技术，并基于过去的观测预测其未来位置[^2]。该方法特别适用于线性动力学系统的状态估计，这类系统具有高斯噪声下的线性动态和观察模型。

#### 高斯分布假设
在卡尔曼滤波框架下，所有的随机变量都假定服从多维正态（高斯）分布。对于隐藏的状态 \(X(t)\)，以及观测到的数据 \(Y(t)\)，它们分别由均值向量 \(\mu\) 和协方差矩阵 \(\Sigma\) 描述：

\[ p(x_t | y_{1:t}) = N(x_t; \hat{x}_t, P_t) \]

其中，\(\hat{x}_t\) 表示时间步 \(t\) 的最优估计值；\(P_t\) 是对应的误差协方差矩阵，反映了不确定性程度。

#### 收敛性和稳定性
当初始条件合理设定时，随着更多测量数据的到来，卡尔曼增益会逐渐减小，使得更新后的状态更加接近真实值。理论上，在理想条件下——即模型参数已知且无建模错误的情况下，经过充分迭代之后，估计过程能够达到稳定收敛于真值附近的小范围内波动。

import numpy as np

def kalman_filter(y, A, B, H, Q, R, x0, P0):
    n = len(A)
    m = len(H)

    # 初始化
    x_est = x0.copy()
    P_est = P0.copy()

    estimates = []
    
    for yt in y:
        # 时间更新 (预测阶段)
        x_pred = A @ x_est
        P_pred = A @ P_est @ A.T + Q
        
        # 测量更新 (校正阶段)
        K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R)
        x_est = x_pred + K @ (yt - H @ x_pred)
        P_est = (np.eye(n) - K @ H) @ P_pred
    
        estimates.append(x_est)
        
    return np.array(estimates)

此代码实现了标准离散时间卡尔曼滤波算法的核心部分，展示了如何通过递推方式计算每次新的观测到来时的最佳状态估计及其不确定度评估。