python实现扩展卡尔曼滤波

扩展卡尔曼滤波（Extended Kalman Filter, EKF）是一种用于非线性系统状态估计的滤波算法。下面是一个使用Python实现的简单示例：

import numpy as np

def extended_kalman_filter(x, P, z, Q, R, F, H):
    # 预测步骤
    x_pred = F @ x
    P_pred = F @ P @ F.T + Q

    # 更新步骤
    y = z - H @ x_pred
    S = H @ P_pred @ H.T + R
    K = P_pred @ H.T @ np.linalg.inv(S)

    x = x_pred + K @ y
    P = (np.eye(len(x)) - K @ H) @ P_pred

    return x, P

# 初始化状态向量和协方差矩阵
x = np.array([0, 0])  # 初始状态
P = np.array([[1, 0], [0, 1]])  # 初始协方差

# 系统噪声和测量噪声的协方差矩阵
Q = np.array([[0.1, 0], [0, 0.1]])  # 系统噪声的协方差
R = np.array([[1, 0], [0, 1]])  # 测量噪声的协方差

# 状态转移矩阵和测量矩阵
F = np.array([[1, 1], [0, 1]])  # 状态转移矩阵
H = np.array([[1, 0], [0, 1]])  # 测量矩阵

# 测量值
z = np.array([2, 2])

# 执行扩展卡尔曼滤波
x, P = extended_kalman_filter(x, P, z, Q, R, F, H)

print("状态估计：", x)
print("协方差矩阵：", P)

在这个示例中，我们首先定义了扩展卡尔曼滤波的函数 `extended_kalman_filter`。然后，我们初始化了状态向量 `x` 和协方差矩阵 `P`，以及系统噪声和测量噪声的协方差矩阵 `Q` 和 `R`。接下来，定义了状态转移矩阵 `F` 和测量矩阵 `H`。

在执行滤波之前，我们需要提供测量值 `z`。然后，调用 `extended_kalman_filter` 函数执行扩展卡尔曼滤波，并得到状态估计值 `x` 和协方差矩阵 `P`。

最后，将状态估计值和协方差矩阵打印出来。注意，这只是一个简单的示例，实际应用中可能需要根据具体问题进行适当的调整和扩展。