二维轨迹的无迹卡尔曼滤波 python

以下是一个简单的 Python 代码示例，用于实现二维轨迹的无迹卡尔曼滤波：

import numpy as np

def unscented_transform(sigma_points, weights):
    mean = np.sum(sigma_points * weights, axis=1)
    covariance = np.zeros((2, 2))
    for i in range(sigma_points.shape[1]):
        diff = sigma_points[:, i] - mean
        covariance += weights[i] * np.outer(diff, diff)
    return mean, covariance

def unscented_kalman_filter(z, dt, Q, R, x0, P0):
    n = len(x0)
    alpha = 1e-3
    beta = 2
    kappa = 0
    lambda_ = alpha**2 * (n + kappa) - n
    c = n + lambda_
    Wm = np.zeros(2 * n + 1)
    Wc = np.zeros(2 * n + 1)
    Wm[0] = lambda_ / c
    Wc[0] = Wm[0] + (1 - alpha**2 + beta)
    for i in range(1, 2 * n + 1):
        Wm[i] = 1 / (2 * c)
        Wc[i] = Wm[i]
    x = x0
    P = P0
    for i in range(len(z)):
        # Generate sigma points
        sigma_points = np.zeros((n, 2 * n + 1))
        sigma_points[:, 0] = x
        for j in range(n):
            sigma_points[:, j+1] = x + np.sqrt(c) * np.sqrt(P[j, j])
            sigma_points[:, j+1+n] = x - np.sqrt(c) * np.sqrt(P[j, j])
        # Propagate sigma points through process model
        x_pred = np.zeros(n)
        for j in range(2 * n + 1):
            x_pred += Wm[j] * process_model(sigma_points[:, j], dt)
        P_pred = np.zeros((n, n))
        for j in range(2 * n + 1):
            diff = process_model(sigma_points[:, j], dt) - x_pred
            P_pred += Wc[j] * np.outer(diff, diff)
        P_pred += Q
        # Propagate sigma points through measurement model
        z_pred = np.zeros(2)
        for j in range(2 * n + 1):
            z_pred += Wm[j] * measurement_model(sigma_points[:, j])
        S = np.zeros((2, 2))
        for j in range(2 * n + 1):
            diff = measurement_model(sigma_points[:, j]) - z_pred
            S += Wc[j] * np.outer(diff, diff)
        S += R
        # Calculate Kalman gain
        K = np.dot(P_pred, np.linalg.inv(S))
        # Update state estimate and covariance
        x = x_pred + np.dot(K, z[i] - z_pred)
        P = P_pred - np.dot(K, np.dot(S, K.T))
    return x, P

def process_model(x, dt):
    # Simple linear process model
    A = np.array([[1, dt], [0, 1]])
    return np.dot(A, x)

def measurement_model(x):
    # Simple linear measurement model
    H = np.array([[1, 0], [0, 1]])
    return np.dot(H, x)

# Example usage
z = np.array([[1, 2], [2, 4], [3, 6], [4, 8], [5, 10]])
dt = 1
Q = np.eye(2) * 0.1
R = np.eye(2) * 1
x0 = np.zeros(2)
P0 = np.eye(2) * 10
x, P = unscented_kalman_filter(z, dt, Q, R, x0, P0)
print(x)
print(P)

这个代码示例中实现了一个简单的二维轨迹的无迹卡尔曼滤波器。在这个例子中，假设我们已经测量了轨迹在时间 $t=1,2,3,4,5$ 时的位置 $(1,2),(2,4),(3,6),(4,8),(5,10)$。我们使用一个简单的线性过程模型（$x_{k+1} = Ax_k$）和线性测量模型（$z_k = Hx_k$），并且假设过程噪声和测量噪声都是高斯分布的。我们使用无迹卡尔曼滤波器来估计轨迹的状态和协方差矩阵。最后，我们输出估计的状态和协方差矩阵。