用python编写的UKF（无迹卡尔曼滤波）代码，状态量和观测量都是三维、非线性的，注释使用中文

用python编写的UKF（无迹卡尔曼滤波）代码，状态量和观测量都是三维、非线性的，注释使用中文

import numpy as np

class UKF:
    def __init__(self, dim_x, dim_z, alpha=1e-3, beta=2, kappa=0):
        self.dim_x = dim_x  # 状态维度
        self.dim_z = dim_z  # 观测维度
        self.alpha = alpha  # 探测精度
        self.beta = beta    # 统计量
        self.kappa = kappa  # 二次调节参数

        # 计算λ和权重
        self.lam = alpha**2 * (dim_x + kappa) - dim_x
        self.gamma = np.sqrt(dim_x + self.lam)  # sigma点扩展因子

        # 权重
        self.Wm = np.full(2 * dim_x + 1, 0.5 / (dim_x + self.lam))
        self.Wm[0] = self.lam / (dim_x + self.lam)  # 第一个sigma点的权重
        self.Wc = np.copy(self.Wm)
        self.Wc[0] += (1 - alpha**2 + beta)  # 计算协方差权重

        self.x = np.zeros(dim_x)  # 初始状态
        self.P = np.eye(dim_x)     # 初始协方差

    def predict(self, dt):
        # 生成sigma点
        sigma_points = self._sigma_points(self.x, self.P)

        # 状态转移函数 (非线性)
        for i in range(sigma_points.shape[0]):
            sigma_points[i] = self._state_transition(sigma_points[i], dt)

        # 计算预测状态和协方差
        self.x = np.dot(self.Wm, sigma_points)  # 预测状态
        self.P = self._update_covariance(sigma_points, self.x)

    def update(self, z):
        # 生成sigma点
        sigma_points = self._sigma_points(self.x, self.P)

        # 观测预测 (非线性)
        z_pred = np.zeros((2 * self.dim_x + 1, self.dim_z))
        for i in range(sigma_points.shape[0]):
            z_pred[i] = self._measurement_function(sigma_points[i])

        # 计算观测均值和协方差
        z_mean = np.dot(self.Wm, z_pred)  # 观测均值
        P_zz = self._update_covariance(z_pred, z_mean)  # 观测协方差

        # 计算交叉协方差
        P_xz = np.zeros((self.dim_x, self.dim_z))
        for i in range(sigma_points.shape[0]):
            P_xz += self.Wc[i] * np.outer(sigma_points[i] - self.x, z_pred[i] - z_mean)

        # 卡尔曼增益
        K = np.dot(P_xz, np.linalg.inv(P_zz))

        # 更新状态和协方差
        self.x += np.dot(K, z - z_mean)
        self.P -= np.dot(K, P_zz).dot(K.T)

    def _sigma_points(self, x, P):
        """生成sigma点"""
        n = self.dim_x
        sigma_points = np.zeros((2 * n + 1, n))
        sigma_points[0] = x
        A = np.linalg.cholesky((n + self.lam) * P)  # Cholesky分解

        for i in range(n):
            sigma_points[i + 1] = x + A[i]
            sigma_points[n + i + 1] = x - A[i]

        return sigma_points

    def _state_transition(self, x, dt):
        """状态转移函数"""
        # 示例：简单的非线性运动模型
        F = np.array([[1, dt, 0],
                      [0, 1, dt],
                      [0, 0, 1]])
        return F @ x  # 状态转移

    def _measurement_function(self, x):
        """观测模型"""
        # 示例：简单的非线性观测模型
        H = np.array([[1, 0, 0],
                      [0, 1, 0],
                      [0, 0, 1]])
        return H @ x  # 观测

    def _update_covariance(self, sigma_points, mean):
        """更新协方差"""
        P = np.zeros((self.dim_x, self.dim_x))
        for i in range(sigma_points.shape[0]):
            P += self.Wc[i] * np.outer(sigma_points[i] - mean, sigma_points[i] - mean)
        return P

# 示例用法
if __name__ == "__main__":
    dt = 0.1  # 时间步长
    ukf = UKF(dim_x=3, dim_z=3)  # 初始化UKF

    # 模拟预测和更新
    for _ in range(100):
        ukf.predict(dt)  # 预测步骤
        # 假设有某个观测值
        z = np.array([1, 0.5, 0.3])  # 示例观测值
        ukf.update(z)  # 更新步骤
        print("当前状态:", ukf.x)

代码讲解

这段代码实现了无迹卡尔曼滤波器（UKF），适用于非线性系统的状态估计。以下是对代码的详细讲解和相关公式的说明：

UKF 类的构造

• 初始化参数：

  • dim_x: 状态空间的维度。
  • dim_z: 观测空间的维度。
  • alpha: 控制 sigma 点的散布程度，通常取小值（如 1e-3）。
  • beta: 用于结合先验分布的统计信息，通常取 2。
  • kappa: 二次调节参数，通常取 0。

• 计算参数：

  • 计算量 λ（lambda）和权重 Wm、Wc，用于生成和加权 sigma 点。
  • λ 的计算公式为：
    $$\lambda ={\alpha }^2(n+\kappa )-n$$
  • Wm 和 Wc 的计算公式为：
    $$Wm[0]=\frac{\lambda }{n+\lambda },Wm[i]=\frac{1}{2(n+\lambda )}(i=1,\dots ,2n)$$
    $$Wc[0]=Wm[0]+(1-{\alpha }^2+\beta ),Wc[i]=Wm[i](i=1,\dots ,2n)$$

预测步骤

• 生成 sigma 点：

  • 使用当前状态和协方差生成 sigma 点，以表示状态的分布。

• 状态转移：

  • 对每个 sigma 点应用非线性状态转移函数，更新状态。

• 计算预测状态和协方差：

  • 通过加权sigma点计算预测状态 $\hat{x}$：
    $$\hat{x}=\sum _{i=0}^{2n}Wm[i]\cdot {\sigma }_i$$
  • 更新协方差 $P$：
    $$P=\sum _{i=0}^{2n}Wc[i]\cdot ({\sigma }_i-\hat{x})\cdot ({\sigma }_i-\hat{x}{)}^T$$

更新步骤

• 生成 sigma 点：

  • 生成当前状态的 sigma 点。

• 观测预测：

  • 对每个 sigma 点应用观测模型，得到预测观测值 ( z_{\text{pred}} )。

• 计算观测均值和协方差：

  • 观测均值$z_{\text{mean}}$：
    $$z_{\text{mean}}=\sum _{i=0}^{2n}Wm[i]\cdot z_{\text{pred},i}$$
  • 观测协方差 $P_{zz}$：
    $$P_{zz}=\sum _{i=0}^{2n}Wc[i]\cdot (z_{\text{pred},i}-z_{\text{mean}})\cdot (z_{\text{pred},i}-z_{\text{mean}}{)}^T$$

• 交叉协方差：

  • 计算状态和观测之间的交叉协方差 $P_{xz}$：
    $$P_{xz}=\sum _{i=0}^{2n}Wc[i]\cdot ({\sigma }_i-\hat{x})\cdot (z_{\text{pred},i}-z_{\text{mean}}{)}^T$$

• 卡尔曼增益：

  • 计算卡尔曼增益 $K$：
    $$K=P_{xz}\cdot P_{zz}^{-1}$$

• 更新状态和协方差：

  • 更新状态：
    $$\hat{x}=\hat{x}+K\cdot (z-z_{\text{mean}})$$
  • 更新协方差：
    $$P=P-K\cdot P_{zz}\cdot K^T$$

辅助函数

• _sigma_points：生成 sigma 点，使用 Cholesky 分解来扩展状态的协方差。
• _state_transition：定义非线性的状态转移函数，示例中使用线性状态转移。
• _measurement_function：定义非线性的观测模型。
• _update_covariance：更新协方差矩阵。

示例用法

代码最后部分展示了如何使用 UKF 类进行状态预测和更新。通过循环模拟 100 次预测和更新，在每次更新中使用示例观测值。

总结

无迹卡尔曼滤波器适用于处理非线性系统，通过生成和加权 sigma 点来有效捕捉状态的分布信息。这段代码提供了一个基本的 UKF 实现，涵盖了状态预测、观测更新和协方差更新的完整流程。

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者