利用python写一个卡尔曼滤波

好的，以下是一个简单的 Python 实现卡尔曼滤波的代码：

import numpy as np

class KalmanFilter:
    def __init__(self, F, H, Q, R, x0, P0):
        self.F = F  # 状态转移矩阵
        self.H = H  # 观测矩阵
        self.Q = Q  # 系统噪声协方差矩阵
        self.R = R  # 观测噪声协方差矩阵
        self.x = x0  # 初始状态
        self.P = P0  # 初始协方差矩阵

    def predict(self, u=0):
        self.x = np.dot(self.F, self.x) + u  # 预测状态
        self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q  # 预测协方差矩阵

    def update(self, z):
        K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(np.dot(np.dot(self.H, self.P), self.H.T) + self.R))  # 计算 Kalman 增益
        self.x = self.x + np.dot(K, z - np.dot(self.H, self.x))  # 更新状态
        self.P = self.P - np.dot(np.dot(K, self.H), self.P)  # 更新协方差矩阵

使用方法：

# 定义状态转移矩阵 F
F = np.array([[1., 1.], [0., 1.]])
# 定义观测矩阵 H
H = np.array([[1., 0.]])
# 定义系统噪声协方差矩阵 Q
Q = np.array([[0.1, 0.], [0., 0.1]])
# 定义观测噪声协方差矩阵 R
R = np.array([[1.]])
# 定义初始状态 x0
x0 = np.array([[0.], [0.]])
# 定义初始协方差矩阵 P0
P0 = np.array([[1., 0.], [0., 1.]])

# 创建 KalmanFilter 对象
kf = KalmanFilter(F, H, Q, R, x0, P0)

# 生成模拟数据
t = np.arange(0, 100)
x_true = np.array([10.0, 1.0])
x = np.zeros((2, len(t)))
z = np.zeros((1, len(t)))
for i in range(len(t)):
    x_true = np.dot(F, x_true) + np.random.multivariate_normal([0, 0], Q)
    x[:, i] = x_true
    z[:, i] = np.dot(H, x_true) + np.random.normal(0, R[0, 0])

# 进行卡尔曼滤波
for i in range(len(t)):
    kf.predict()
    kf.update(z[:, i])

# 绘制结果
import matplotlib.pyplot as plt

plt.figure()
plt.plot(t, x[0, :], label='True position')
plt.plot(t, z[0, :], label='Measured position')
plt.plot(t, kf.x[0, :], label='Filtered position')
plt.legend()
plt.show()

这个例子中，我们生成了一些模拟数据，然后使用 KalmanFilter 对象进行卡尔曼滤波，最后绘制了真实位置、测量位置和滤波位置的图形。