卡尔曼滤波算法python

卡尔曼滤波算法是一种用于状态估计的算法，可以根据系统模型和观测数据，对系统的状态进行估计和预测。下面是一个用Python实现卡尔曼滤波算法的示例代码：

import numpy as np
import matplotlib.pyplot as plt

# 状态转移矩阵
A = np.array([[1, 1], [0, 1]])
# 观测矩阵
H = np.array([[1, 0]])
# 初始状态
x0 = np.array([0, 0])
# 初始协方差矩阵
P0 = np.array([[1, 0], [0, 1]])
# 过程噪声协方差矩阵
Q = np.array([[0.01, 0], [0, 0.01]])
# 观测噪声协方差矩阵
R = np.array([[1]])

# 生成随机数据
np.random.seed(0)
n = 100
t = np.arange(n)
x = np.zeros((2, n))
y = np.zeros((1, n))
x[:, 0] = x0
for i in range(1, n):
    x[:, i] = np.dot(A, x[:, i-1]) + np.random.multivariate_normal([0, 0], Q)
    y[:, i] = np.dot(H, x[:, i]) + np.random.normal(0, R)

# 卡尔曼滤波算法
xhat = np.zeros((2, n))
xhat[:, 0] = x0
Phat = np.zeros((2, 2, n))
Phat[:, :, 0] = P0
for i in range(1, n):
    # 预测
    xhat[:, i] = np.dot(A, xhat[:, i-1])
    Phat[:, :, i] = np.dot(np.dot(A, Phat[:, :, i-1]), A.T) + Q
    # 更新
    K = np.dot(np.dot(Phat[:, :, i], H.T), np.linalg.inv(np.dot(np.dot(H, Phat[:, :, i]), H.T) + R))
    xhat[:, i] = xhat[:, i] + np.dot(K, y[:, i] - np.dot(H, xhat[:, i]))
    Phat[:, :, i] = np.dot(np.eye(2) - np.dot(K, H), Phat[:, :, i])

# 绘制结果
plt.figure()
plt.plot(t, x[0, :], 'r-', label='real')
plt.plot(t, y[0, :], 'b.', label='measurement')
plt.plot(t, xhat[0, :], 'g-', label='estimate')
plt.legend()
plt.title('Position')
plt.xlabel('Time')
plt.ylabel('Position')

plt.figure()
plt.plot(t, x[1, :], 'r-', label='real')
plt.plot(t, xhat[1, :], 'g-', label='estimate')
plt.legend()
plt.title('Velocity')
plt.xlabel('Time')
plt.ylabel('Velocity')

plt.show()

在这个示例中，我们使用了一个简单的状态转移矩阵和观测矩阵，通过生成随机数据模拟了一个系统的状态和观测数据，然后使用卡尔曼滤波算法对其进行估计。最终，我们绘制了实际状态、观测数据和估计状态的图像，可以看到估计结果与实际状态比较接近。