python二维卡尔曼滤波代码

根据提供的引用内容，以下是一个使用Python实现的二维卡尔曼滤波的示例代码：

import numpy as np

def kalman_filter(x, P, measurements, R, A, B, u):
    for n in range(len(measurements)):
        # 预测步骤
        x = np.dot(A, x) + np.dot(B, u)
        P = np.dot(np.dot(A, P), A.T)
        
        # 更新步骤
        y = measurements[n] - np.dot(H, x)
        S = np.dot(np.dot(H, P), H.T) + R
        K = np.dot(np.dot(P, H.T), np.linalg.inv(S))
        x = x + np.dot(K, y)
        P = np.dot((I - np.dot(K, H)), P)
        
    return x, P

# 初始化参数
dt = 0.1
x = np.array([[0.0], [0.0]])  # 初始状态向量
P = np.array([[1000.0, 0.0], [0.0, 1000.0]])  # 初始协方差矩阵
A = np.array([[1.0, dt], [0.0, 1.0]])  # 状态转移矩阵
B = np.array([[0.5*dt**2], [dt]])  # 控制输入矩阵
u = np.array([[0.0]])  # 控制输入向量
H = np.array([[1.0, 0.0]])  # 观测矩阵
R = np.array([[1.0]])  # 观测噪声协方差矩阵
I = np.array([[1.0, 0.0], [0.0, 1.0]])  # 单位矩阵

# 测量数据
measurements = [1, 2, 3, 4, 5]

# 执行卡尔曼滤波
x, P = kalman_filter(x, P, measurements, R, A, B, u)

# 输出结果
print("最终状态向量 x:")
print(x)
print("最终协方差矩阵 P:")
print(P)