扩展卡尔曼滤波python代码

以下是一个简单的扩展卡尔曼滤波的 Python 代码示例，用于演示如何使用扩展卡尔曼滤波来估计一个物体的位置和速度：

import numpy as np

def EKF(x, P, F, Q, H, R, z):
    # 预测步骤
    x = np.dot(F, x)
    P = np.dot(np.dot(F, P), F.T) + Q

    # 更新步骤
    S = np.dot(np.dot(H, P), H.T) + R
    K = np.dot(np.dot(P, H.T), np.linalg.inv(S))
    y = z - np.dot(H, x)
    x = x + np.dot(K, y)
    P = np.dot((np.eye(len(x)) - np.dot(K, H)), P)

    return x, P

# 初始化状态向量、协方差矩阵和测量矩阵
x = np.array([0, 0, 0, 0])  # [位置, 速度, 加速度, 偏移]
P = np.diag([100, 10, 1, 0.1])
H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])

# 初始化过程噪声和测量噪声的协方差矩阵
Q = np.diag([0.1, 0.01, 0.001, 0.0001])
R = np.diag([10, 1])

# 初始化状态转移矩阵
dt = 0.1
F = np.array([[1, dt, 0.5*dt**2, 0], [0, 1, dt, 0], [0, 0, 1, 0], [0, 0, 0, 1]])

# 模拟测量
t = np.arange(0, 10, dt)
measurements = np.sin(t) + np.random.randn(len(t))

# 运行扩展卡尔曼滤波
for i in range(len(t)):
    z = np.array([measurements[i], 0])
    x, P = EKF(x, P, F, Q, H, R, z)
    print("Time step: ", i)
    print("Position: ", x[0])
    print("Velocity: ", x[1])
    print()

在这个示例中，我们使用扩展卡尔曼滤波来估计一个物体的位置和速度。我们使用一个简单的状态向量 `[位置, 速度, 加速度, 偏移]`，其中 `加速度` 和 `偏移` 是我们用来估计位置和速度的额外状态。

我们使用一个简单的状态转移矩阵来模拟物体的运动，并使用正弦函数模拟测量。在每个时间步骤中，我们使用扩展卡尔曼滤波来估计物体的状态，并打印出位置和速度的估计值。