python 卡尔曼滤波

import numpy as np
from filterpy.kalman import KalmanFilter

# 创建一个一维卡尔曼滤波器
kf = KalmanFilter(dim_x=2, dim_z=1)

# 初始化状态向量 [位置, 速度]
kf.x = np.array([0., 0.])

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

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

# 过程噪声协方差矩阵 Q
kf.Q = np.array([[1e-5, 0.],
                 [0., 1e-4]])

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

# 初始估计误差协方差矩阵 P
kf.P = np.array([[1000., 0.],
                 [0., 1000.]])

# 模拟数据
zs = np.random.normal(0, 1, size=(100))  # 生成100个观测值

# 存储预测和更新后的状态
means = []
for z in zs:
    kf.predict()
    kf.update(z)
    means.append(kf.x)

# 打印最终的状态估计
print("Final state estimate:", kf.x)

解释说明：

1. KalmanFilter 类：使用 filterpy 库中的 KalmanFilter 类来创建卡尔曼滤波器。
2. 初始化：
   • dim_x 表示状态向量的维度（这里是2，表示位置和速度）。
   • dim_z 表示观测向量的维度（这里是1，表示只观测位置）。
3. 状态向量 x：初始状态为 [位置, 速度]，这里都设为0。
4. 状态转移矩阵 F：描述系统状态如何从一个时间步转移到下一个时间步。
5. 观测矩阵 H：将状态向量映射到观测空间。
6. 过程噪声协方差矩阵 Q：描述模型的不确定性。
7. 测量噪声协方差矩阵 R：描述观测的不确定性。
8. 初始估计误差协方差矩阵 P：表示对初始状态估计的不确定度。
9. 模拟数据：生成一些随机的观测值。
10. 预测和更新：通过 predict() 和 update() 方法进行卡尔曼滤波的预测和更新步骤。
11. 结果：打印最终的状态估计。