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

用python编写的UKF(无迹卡尔曼滤波)代码,状态量和观测量都是三维、非线性的,注释使用中文_第1张图片

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

文章目录

  • 代码讲解
    • 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)和权重 WmWc,用于生成和加权 sigma 点。
    • λ 的计算公式为:
      λ = α 2 ( n + κ ) − n \lambda = \alpha^2 (n + \kappa) - n λ=α2(n+κ)n
    • WmWc 的计算公式为:
      W m [ 0 ] = λ n + λ , W m [ i ] = 1 2 ( n + λ ) ( i = 1 , … , 2 n ) Wm[0] = \frac{\lambda}{n + \lambda}, \quad Wm[i] = \frac{1}{2(n + \lambda)} \quad (i = 1, \ldots, 2n) Wm[0]=n+λλ,Wm[i]=2(n+λ)1(i=1,,2n)
      W c [ 0 ] = W m [ 0 ] + ( 1 − α 2 + β ) , W c [ i ] = W m [ i ] ( i = 1 , … , 2 n ) Wc[0] = Wm[0] + (1 - \alpha^2 + \beta), \quad Wc[i] = Wm[i] \quad (i = 1, \ldots, 2n) Wc[0]=Wm[0]+(1α2+β),Wc[i]=Wm[i](i=1,,2n)

预测步骤

  • 生成 sigma 点

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

    • 对每个 sigma 点应用非线性状态转移函数,更新状态。
  • 计算预测状态和协方差

    • 通过加权sigma点计算预测状态 x ^ \hat{x} x^
      x ^ = ∑ i = 0 2 n W m [ i ] ⋅ σ i \hat{x} = \sum_{i=0}^{2n} Wm[i] \cdot \sigma_i x^=i=02nWm[i]σi
    • 更新协方差 P P P
      P = ∑ i = 0 2 n W c [ i ] ⋅ ( σ i − x ^ ) ⋅ ( σ i − x ^ ) T P = \sum_{i=0}^{2n} Wc[i] \cdot (\sigma_i - \hat{x}) \cdot (\sigma_i - \hat{x})^T P=i=02nWc[i](σix^)(σix^)T

更新步骤

  • 生成 sigma 点

    • 生成当前状态的 sigma 点。
  • 观测预测

    • 对每个 sigma 点应用观测模型,得到预测观测值 ( z_{\text{pred}} )。
  • 计算观测均值和协方差

    • 观测均值 z mean z_{\text{mean}} zmean
      z mean = ∑ i = 0 2 n W m [ i ] ⋅ z pred , i z_{\text{mean}} = \sum_{i=0}^{2n} Wm[i] \cdot z_{\text{pred},i} zmean=i=02nWm[i]zpred,i
    • 观测协方差 P z z P_{zz} Pzz
      P z z = ∑ i = 0 2 n W c [ i ] ⋅ ( z pred , i − z mean ) ⋅ ( z pred , i − z mean ) T 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 Pzz=i=02nWc[i](zpred,izmean)(zpred,izmean)T
  • 交叉协方差

    • 计算状态和观测之间的交叉协方差 P x z P_{xz} Pxz
      P x z = ∑ i = 0 2 n W c [ i ] ⋅ ( σ i − x ^ ) ⋅ ( z pred , i − z mean ) T P_{xz} = \sum_{i=0}^{2n} Wc[i] \cdot (\sigma_i - \hat{x}) \cdot (z_{\text{pred},i} - z_{\text{mean}})^T Pxz=i=02nWc[i](σix^)(zpred,izmean)T
  • 卡尔曼增益

    • 计算卡尔曼增益 K K K
      K = P x z ⋅ P z z − 1 K = P_{xz} \cdot P_{zz}^{-1} K=PxzPzz1
  • 更新状态和协方差

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

辅助函数

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

示例用法

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

总结

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

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

你可能感兴趣的:(卡尔曼与python,python,开发语言)