帮我写一个扩展卡尔曼滤波器

扩展卡尔曼滤波器（Extended Kalman Filter，EKF）是一种基于卡尔曼滤波器的扩展版本，用于非线性系统的状态估计。它通过线性化非线性系统模型来近似系统的真实动态，并在卡尔曼滤波器的框架下进行状态估计。下面是一个简单的扩展卡尔曼滤波器的实现。

假设我们有一个非线性系统，其状态变量为$x_k$，状态转移方程为：

$$x_k = f(x_{k-1}, u_{k-1}) + w_{k-1}$$

其中，$u_{k-1}$是控制输入，$w_{k-1}$是系统噪声。我们可以通过线性化$f(\cdot)$来近似非线性系统：

$$x_k \approx f(\hat{x}_{k-1}, u_{k-1}) + F_{k-1}(\hat{x}_{k-1}, u_{k-1})(x_{k-1} - \hat{x}_{k-1}) + w_{k-1}$$

其中，$\hat{x}_{k-1}$是上一时刻的状态估计值，$F_{k-1}$是$f(\cdot)$在$\hat{x}_{k-1}$处的雅可比矩阵。我们可以使用扩展卡尔曼滤波器来估计$x_k$的值。

扩展卡尔曼滤波器的状态估计步骤如下：

1. 预测状态：$x_k^- = f(\hat{x}_{k-1}, u_{k-1})$
2. 预测误差协方差矩阵：$P_k^- = F_{k-1}P_{k-1}F_{k-1}^T + Q_{k-1}$
3. 计算卡尔曼增益：$K_k = P_k^-H_k^T(H_kP_k^-H_k^T + R_k)^{-1}$
4. 测量更新状态：$\hat{x}_k = x_k^- + K_k(z_k - h(x_k^-))$
5. 更新误差协方差矩阵：$P_k = (I - K_kH_k)P_k^-$

其中，$z_k$是当前时刻的测量值，$h(\cdot)$是测量函数，$H_k$是$h(\cdot)$在$x_k^-$处的雅可比矩阵，$Q_k$和$R_k$分别是系统噪声和测量噪声的协方差矩阵。

以下是一个Python实现的扩展卡尔曼滤波器：

import numpy as np

class EKF:
    def __init__(self, f, h, F, H, Q, R, x0, P0):
        self.f = f
        self.h = h
        self.F = F
        self.H = H
        self.Q = Q
        self.R = R
        self.x = x0
        self.P = P0

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

    def update(self, z):
        # 计算卡尔曼增益
        K = np.dot(np.dot(self.P, self.H(self.x)), np.linalg.inv(np.dot(np.dot(self.H(self.x), self.P), self.H(self.x).T) + self.R))
        # 测量更新状态
        self.x = self.x + np.dot(K, (z - self.h(self.x)))
        # 更新误差协方差矩阵
        self.P = np.dot((np.eye(self.P.shape[0]) - np.dot(K, self.H(self.x))), self.P)

    def run(self, u, z):
        self.predict(u)
        self.update(z)
        return self.x

其中，$f$和$h$分别是状态转移函数和测量函数，$F$和$H$是它们在当前状态处的雅可比矩阵，$Q$和$R$分别是系统噪声和测量噪声的协方差矩阵，$x0$和$P0$是初始状态和误差协方差矩阵。`predict`方法用于预测状态，`update`方法用于测量更新状态，`run`方法用于一次完整的滤波。