扩展卡尔曼滤波的python实现

下面是一个简单的扩展卡尔曼滤波的Python实现示例：

import numpy as np

class ExtendedKalmanFilter:
    
    def __init__(self, state_dim, obs_dim, f_func, h_func, Q, R, P0):
        """
        state_dim: 状态向量的维度
        obs_dim: 观测向量的维度
        f_func: 状态转移函数，接受一个状态向量作为输入，返回一个新的状态向量
        h_func: 观测函数，接受一个状态向量作为输入，返回一个观测向量
        Q: 状态噪声的协方差矩阵
        R: 观测噪声的协方差矩阵
        P0: 初始状态的协方差矩阵
        """
        self.state_dim = state_dim
        self.obs_dim = obs_dim
        self.f_func = f_func
        self.h_func = h_func
        self.Q = Q
        self.R = R
        self.P = P0
        self.x = np.zeros((state_dim, 1))
    
    def predict(self):
        """
        预测状态和协方差矩阵
        """
        # 1. 预测状态
        self.x = self.f_func(self.x)
        
        # 2. 计算状态转移矩阵
        F = self.jacobian(self.f_func, self.x)
        
        # 3. 预测协方差矩阵
        self.P = F @ self.P @ F.T + self.Q
    
    def update(self, z):
        """
        根据观测更新状态和协方差矩阵
        """
        # 1. 计算观测向量的预测值
        z_pred = self.h_func(self.x)
        
        # 2. 计算观测矩阵
        H = self.jacobian(self.h_func, self.x)
        
        # 3. 计算卡尔曼增益
        K = self.P @ H.T @ np.linalg.inv(H @ self.P @ H.T + self.R)
        
        # 4. 更新状态和协方差矩阵
        self.x = self.x + K @ (z - z_pred)
        self.P = (np.eye(self.state_dim) - K @ H) @ self.P
        
    def jacobian(self, func, x):
        """
        计算函数在指定点的雅可比矩阵
        """
        eps = 1e-6
        J = np.zeros((self.obs_dim, self.state_dim))
        for i in range(self.state_dim):
            x1 = np.copy(x)
            x2 = np.copy(x)
            x1[i][0] += eps
            x2[i][0] -= eps
            J[:,i] = (func(x1) - func(x2))[:,0] / (2 * eps)
        return J

使用方法如下：

# 定义状态转移函数和观测函数
def f(x):
    return np.array([[x[0][0] + x[1][0]], [x[1][0]]])

def h(x):
    return np.array([[x[0][0]]])

# 定义初始状态、协方差矩阵、噪声协方差矩阵
x0 = np.array([[0], [0]])
P0 = np.eye(2)
Q = np.eye(2) * 0.01
R = np.eye(1) * 0.1

# 创建扩展卡尔曼滤波器
ekf = ExtendedKalmanFilter(2, 1, f, h, Q, R, P0)

# 模拟观测
x_true = np.zeros((2, 1))
z = []
for i in range(100):
    x_true = f(x_true) + np.random.normal(size=(2,1)) * np.sqrt(Q)
    z.append(h(x_true) + np.random.normal(size=(1,1)) * np.sqrt(R))

# 使用扩展卡尔曼滤波器进行滤波
x_filt = []
for i in range(100):
    ekf.predict()
    ekf.update(z[i])
    x_filt.append(ekf.x.copy())

这个例子中，我们使用扩展卡尔曼滤波器对一个一维随机游走进行滤波。实际上，这个例子中的扩展卡尔曼滤波器非常简单，因为状态转移函数和观测函数都是线性的，不需要使用非线性的状态转移和观测函数。但是，这个例子可以帮助我们理解扩展卡尔曼滤波器的基本思想。