卡尔曼滤波算法python

卡尔曼滤波是一种用于估计线性动态系统状态的算法，可以用于处理传感器测量误差等问题。下面是使用Python实现简单的卡尔曼滤波算法的示例代码：

import numpy as np

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

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

# 系统噪声协方差矩阵
Q = np.array([[0.0001, 0], [0, 0.0001]])

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

# 初始状态向量
x0 = np.array([[0], [0]])

# 初始状态协方差矩阵
P0 = np.array([[1, 0], [0, 1]])

# 测量值
measurements = [1.23, 1.26, 1.21, 1.22, 1.24]

# 卡尔曼滤波
x_k = x0
P_k = P0
for z_k in measurements:
    # 预测步骤
    x_k = np.dot(A, x_k)
    P_k = np.dot(np.dot(A, P_k), A.T) + Q

    # 更新步骤
    K_k = np.dot(np.dot(P_k, H.T), np.linalg.inv(np.dot(np.dot(H, P_k), H.T) + R))
    x_k = x_k + np.dot(K_k, (z_k - np.dot(H, x_k)))
    P_k = np.dot((np.eye(2) - np.dot(K_k, H)), P_k)

    print(x_k)

这个例子中，我们使用了一维的测量值来估计二维状态值。在每个时间步骤中，我们先进行预测步骤，然后使用测量值进行更新步骤，最后输出状态估计值。