前言

0 数学冷知识

0-1 雅可比矩阵（Jacobian）
  • 在多元微积分中，设向量值函数： f : R n → R m , y = f ( x ) \mathbf{f}: \mathbb{R}^n \rightarrow \mathbb{R}^m,\quad \mathbf{y} = \mathbf{f}(\mathbf{x}) f:Rn→Rm,y=f(x)
  • 其中：
    • x = [ x 1 x 2 ⋮ x n ] , y = [ y 1 y 2 ⋮ y m ] \mathbf{x} = \begin{bmatrix} x_1 \\ x_2 \\ \vdots \\ x_n \end{bmatrix}, \quad \mathbf{y} = \begin{bmatrix} y_1 \\ y_2 \\ \vdots \\ y_m \end{bmatrix} x= ​x1​x2​⋮xn​​ ​,y= ​y1​y2​⋮ym​​ ​
  • 那么有函数 f \mathbf{f} f在点 x \mathbf{x} x 的雅可比矩阵定义为： J f ( x ) = [ ∂ f 1 ∂ x 1 ∂ f 1 ∂ x 2 ⋯ ∂ f 1 ∂ x n ∂ f 2 ∂ x 1 ∂ f 2 ∂ x 2 ⋯ ∂ f 2 ∂ x n ⋮ ⋮ ⋱ ⋮ ∂ f m ∂ x 1 ∂ f m ∂ x 2 ⋯ ∂ f m ∂ x n ] J_{\mathbf{f}}(\mathbf{x}) = \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2} & \cdots & \frac{\partial f_1}{\partial x_n} \\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} & \cdots & \frac{\partial f_2}{\partial x_n} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial f_m}{\partial x_1} & \frac{\partial f_m}{\partial x_2} & \cdots & \frac{\partial f_m}{\partial x_n} \end{bmatrix} Jf​(x)= ​∂x1​∂f1​​∂x1​∂f2​​⋮∂x1​∂fm​​​∂x2​∂f1​​∂x2​∂f2​​⋮∂x2​∂fm​​​⋯⋯⋱⋯​∂xn​∂f1​​∂xn​∂f2​​⋮∂xn​∂fm​​​ ​
  • 雅可比矩阵是多变量向量函数在某一点的一阶全微分线性映射表示
  • 说人话就是：
    • 多变量函数的一阶导数矩阵（导数的矩阵版）
  • 几何意义：
    • 在某一点附近，非线性函数曲面可以被一个线性超平面（切空间）近似
    • 这个切空间的方向变化关系由雅可比矩阵决定
    • 它描述了函数在该点的“局部线性结构”
0-2 泰勒展开

  • 在数学分析中，泰勒展开用于描述一个函数在某一点附近的局部近似行为。

  • 对于一元函数 f ( x ) f(x) f(x)，在点 x 0 x_0 x0​附近的一阶泰勒展开为： f ( x ) ≈ f ( x 0 ) + f ′ ( x 0 ) ( x − x 0 ) f(x) \approx f(x_0) + f'(x_0)(x - x_0) f(x)≈f(x0​)+f′(x0​)(x−x0​)

  • 推广到多元向量函数： f : R n → R m \mathbf{f}: \mathbb{R}^n \rightarrow \mathbb{R}^m f:Rn→Rm则在点 x 0 \mathbf{x}_0 x0​​ 附近的一阶泰勒展开为： f ( x ) ≈ f ( x 0 ) + J f ( x 0 )   ( x − x 0 ) \mathbf{f}(\mathbf{x}) \approx \mathbf{f}(\mathbf{x}_0) + J_{\mathbf{f}}(\mathbf{x}_0)\,(\mathbf{x} - \mathbf{x}_0) f(x)≈f(x0​)+Jf​(x0​)(x−x0​)其中：

  • J f ( x 0 ) J_{\mathbf{f}}(\mathbf{x}_0) Jf​(x0​) 是函数在 x 0 \mathbf{x}_0 x0​ 处的雅可比矩阵

  • ( x − x 0 ) (\mathbf{x} - \mathbf{x}_0) (x−x0​)是输入的微小扰动

  • 本质上：

    • 用线性函数去逼近非线性曲面在局部的行为
  • 多元非线性函数，在局部可以用“线性函数 + 雅可比矩阵”来近似
0-3 平移矩阵与旋转矩阵
  • 在二维空间中，一个点的平移可以表示为： p ′ = [ x ′ y ′ ] = [ x y ] + [ t x t y ] \mathbf{p}' = \begin{bmatrix} x' \\ y' \end{bmatrix} = \begin{bmatrix} x \\ y \end{bmatrix} + \begin{bmatrix} t_x \\ t_y \end{bmatrix} p′=[x′y′​]=[xy​]+[tx​ty​​]
  • 在二维空间中，一个点绕原点旋转角度 t h e t a theta theta 的变换为： R ( θ ) = [ cos ⁡ θ − sin ⁡ θ sin ⁡ θ cos ⁡ θ ] R(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix} R(θ)=[cosθsinθ​−sinθcosθ​]

1 回顾

1-1 卡尔曼滤波的局限性
  • 在上一期我们推导的卡尔曼滤波中，一个核心前提是：系统必须是线性的 x k = F x k − 1 + w k z k = H x k + v k \begin{align} x_k &= F x_{k-1} + w_k \\ z_k &= H x_k + v_k \end{align} xk​zk​​=Fxk−1​+wk​=Hxk​+vk​​​
  • 但现实问题中，大量系统都是非线性的，例如：
    • 机器人运动模型（差速 / 阿克曼）
    • 雷达 / 激光观测模型（极坐标）
    • IMU + GPS 融合
    • 目标跟踪（角度 + 距离）
  • 这时系统通常写成： x k = f ( x k − 1 ) + w k z k = h ( x k ) + v k \begin{align} x_k &= f(x_{k-1}) + w_k \\ z_k &= h(x_k) + v_k \end{align} xk​zk​​=f(xk−1​)+wk​=h(xk​)+vk​​​其中：
  • f ( ⋅ ) f(\cdot) f(⋅)：非线性状态转移函数
  • h ( ⋅ ) h(\cdot) h(⋅)：非线性观测函数
  • 这时候KF依赖的核心假设被破坏了：高斯分布在非线性变换下不再保持高斯。
1-2 扩展
  • 为此，我们有几种解决这个问题的方法
    • 扩展卡尔曼滤波（EKF）：基于一阶泰勒展开的线性化方法
    • 无迹卡尔曼滤波（UKF）：基于Sigma点传播的高阶统计近似方法
    • 粒子滤波（Particle Filter）：基于蒙特卡洛采样与重要性重采样（Sequential Monte Carlo）的方法，用一组离散样本近似任意概率分布

2 EKF 扩展卡尔曼滤波

2-1 核心思想
  • EKF（Extended Kalman Filter）的核心非常简单：

用一阶泰勒展开（线性化）替代非线性函数

f ( x ) ≈ f ( x ^ ) + F ( x − x ^ ) h ( x ) ≈ h ( x ^ ) + H ( x − x ^ ) \begin{align} f(x) & \approx f(\hat{x}) + F(x - \hat{x}) \\ h(x) &\approx h(\hat{x}) + H(x - \hat{x}) \end{align} f(x)h(x)​≈f(x^)+F(x−x^)≈h(x^)+H(x−x^)​​其中：

  • F = ∂ f ∂ x ∣ x ^ F = \frac{\partial f}{\partial x}\big|_{\hat{x}} F=∂x∂f​ ​x^​为状态转移雅可比
  • H = ∂ h ∂ x ∣ x ^ H = \frac{\partial h}{\partial x}\big|_{\hat{x}} H=∂x∂h​ ​x^​为观测雅可比
2-2 推导
  • 我们在上一篇文章中已经详细的推到过卡尔曼滤波其实核心本质就是利用贝叶斯公式对后验概率进行递推更新，从而不断修正状态估计：
    • 预测步骤：利用系统模型传播先验分布
    • 更新步骤：利用观测信息修正后验分布
  • 本节我们来推导EKF 扩展卡尔曼滤波，大致的步骤仍然遵循KF利用贝叶斯公式计算后验频率 p ( x k ∣ z 1 : k ) p(x_k \mid z_{1:k}) p(xk​∣z1:k​)一样：
    • 预测 p ( x k ∣ z 1 : k − 1 ) = ∫ p ( x k ∣ x k − 1 )   p ( x k − 1 ∣ z 1 : k − 1 )   d x k − 1 p(x_k \mid z_{1:k-1}) = \int p(x_k \mid x_{k-1}) \, p(x_{k-1} \mid z_{1:k-1}) \, dx_{k-1} p(xk​∣z1:k−1​)=∫p(xk​∣xk−1​)p(xk−1​∣z1:k−1​)dxk−1​
    • 更新 p ( x k ∣ z 1 : k ) ∝ p ( z k ∣ x k )   p ( x k ∣ z 1 : k − 1 ) p(x_k \mid z_{1:k}) \propto p(z_k \mid x_k)\, p(x_k \mid z_{1:k-1}) p(xk​∣z1:k​)∝p(zk​∣xk​)p(xk​∣z1:k−1​)
2-2-1 系统描述
  • 对于非线性系统，我们有 x k = f ( x k − 1 ) + w k , w k ∼ N ( 0 , Q ) z k = h ( x k ) + v k , v k ∼ N ( 0 , R ) ) \begin{align} x_k & = f(x_{k-1}) + w_k,\quad w_k \sim \mathcal{N}(0,Q) \\ z_k &= h(x_k) + v_k,\quad v_k \sim \mathcal{N}(0,R)) \end{align} xk​zk​​=f(xk−1​)+wk​,wk​∼N(0,Q)=h(xk​)+vk​,vk​∼N(0,R))​​
  • 其中：
    • f ( ⋅ ) f(\cdot) f(⋅)：非线性状态转移矩阵
    • h ( ⋅ ) h(\cdot) h(⋅)：非线性观测矩阵
    • w k w_k wk​：过程噪声，满足高斯分布
    • v k v_k vk​：观测噪声，满足高斯分布
2-2-2 线性化系统
  • 状态方程： x k = f ( x k − 1 ) + w k x_k = f(x_{k-1}) + w_k xk​=f(xk−1​)+wk​
  • 其中在 x ^ k − 1 \hat{x}_{k-1} x^k−1​附近对状态函数线性化进行一阶泰勒展开： f ( x k − 1 ) ≈ f ( x ^ k − 1 ) + F k ( x k − 1 − x ^ k − 1 ) f(x_{k-1}) \approx f(\hat{x}_{k-1}) + F_k (x_{k-1} - \hat{x}_{k-1}) f(xk−1​)≈f(x^k−1​)+Fk​(xk−1​−x^k−1​)
  • 其中： F k = ∂ f ∂ x ∣ x ^ k − 1 F_k = \frac{\partial f}{\partial x}\Big|_{\hat{x}_{k-1}} Fk​=∂x∂f​ ​x^k−1​​

  • 观测方程： z k = h ( x k ) + v k z_k = h(x_k) + v_k zk​=h(xk​)+vk​

  • 其中在 x ^ k ∣ k − 1 \hat{x}_{k|k-1} x^k∣k−1​附近对观测函数进行线性化 h ( x k ) ≈ h ( x ^ k ∣ k − 1 ) + H k ( x k − x ^ k ∣ k − 1 ) h(x_k) \approx h(\hat{x}_{k|k-1}) + H_k (x_k - \hat{x}_{k|k-1}) h(xk​)≈h(x^k∣k−1​)+Hk​(xk​−x^k∣k−1​)

  • 其中： H k = ∂ h ∂ x ∣ x ^ k ∣ k − 1 H_k = \frac{\partial h}{\partial x}\Big|_{\hat{x}_{k|k-1}} Hk​=∂x∂h​ ​x^k∣k−1​​

2-2-3 预测
  • 状态预测 x ^ k ∣ k − 1 = f ( x ^ k − 1 ) \hat{x}_{k|k-1} = f(\hat{x}_{k-1}) x^k∣k−1​=f(x^k−1​)
  • 协方差预测 P k ∣ k − 1 = F k P k − 1 F k T + Q P_{k|k-1} = F_k P_{k-1} F_k^T + Q Pk∣k−1​=Fk​Pk−1​FkT​+Q
  • 这块这两个公式和原本的KF形式一样，故不多介绍（忘记的可以回看上一节的2-3-1 状态预测（Prior））
2-2-4 残差构造

  • 构造观测残差（innovation），和KF一样的形式： y k = z k − h ( x ^ k ∣ k − 1 ) y_k = z_k - h(\hat{x}_{k|k-1}) yk​=zk​−h(x^k∣k−1​)

  • 然后把线性化后的 h ( x k ) h(x_k) h(xk​)带入到观测方程 z k = h ( x k ) + v k z_k=h(x_k)+v_k zk​=h(xk​)+vk​有： z k ≈ h ( x ^ k ∣ k − 1 ) + H k ( x k − x ^ k ∣ k − 1 ) + v k z_k \approx h(\hat{x}_{k|k-1}) + H_k (x_k - \hat{x}_{k|k-1}) + v_k zk​≈h(x^k∣k−1​)+Hk​(xk​−x^k∣k−1​)+vk​

  • 从而得到残差的表达式为： y k ≈ H k ( x k − x ^ k ∣ k − 1 ) + v k y_k \approx H_k (x_k - \hat{x}_{k|k-1}) + v_k yk​≈Hk​(xk​−x^k∣k−1​)+vk​

    • 这里忽略二阶及以上泰勒残差项（small error assumption）
2-2-5 观测协方差计算
  • 根据上一节的结果我们定义 x ~ k = x k − x ^ k ∣ k − 1 \tilde{x}_k = x_k - \hat{x}_{k|k-1} x~k​=xk​−x^k∣k−1​
  • 则残差的表达式为 y k ≈ H k x ~ k + v k y_k\approx H_k \tilde{x}_k + v_k yk​≈Hk​x~k​+vk​
  • 因此我们要计算的协方差就是 S k = E [ y k y k T ] S_k = \mathbb{E}[y_k y_k^T] Sk​=E[yk​ykT​]
  • 我们吧残差展开就有： S k = E [ ( H k x ~ k + v k ) ( H k x ~ k + v k ) T ] S_k = \mathbb{E}[(H_k \tilde{x}_k + v_k)(H_k \tilde{x}_k + v_k)^T] Sk​=E[(Hk​x~k​+vk​)(Hk​x~k​+vk​)T]
  • 这么我们假设 x ~ k ∼ ( 0 , P k ∣ k − 1 ) \tilde{x}_k \sim (0, P_{k|k-1}) x~k​∼(0,Pk∣k−1​)、 v k ∼ ( 0 , R ) v_k \sim (0, R) vk​∼(0,R)这个两者相对对立，我们就可以展开期望： S k = H k P k ∣ k − 1 H k T + R S_k = H_k P_{k|k-1} H_k^T + R Sk​=Hk​Pk∣k−1​HkT​+R
2-2-5 更新
  • Kalman Gain与标准KF一致： K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R ) − 1 K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R)^{-1} Kk​=Pk∣k−1​HkT​(Hk​Pk∣k−1​HkT​+R)−1
  • 也就是 K k = P k ∣ k − 1 H T S k − 1 K_k = P_{k|k-1} H^T S_k^{-1} Kk​=Pk∣k−1​HTSk−1​
  • 状态更新 x ^ k = x ^ k ∣ k − 1 + K k ( z k − h ( x ^ k ∣ k − 1 ) ) \hat{x}_k = \hat{x}_{k|k-1} + K_k \big(z_k - h(\hat{x}_{k|k-1})\big) x^k​=x^k∣k−1​+Kk​(zk​−h(x^k∣k−1​))
  • 协方差更新 P k = ( I − K k H k ) P k ∣ k − 1 P_k = (I - K_k H_k) P_{k|k-1} Pk​=(I−Kk​Hk​)Pk∣k−1​
2-3 简单实现
  • 我们设计一个完全的圆周运动如下：
x = 50 * cos(t)
y = 50 * sin(t)
  • 为此我们同样设计状态空间为 x = [ x y v x v y ] x = \begin{bmatrix} x \\ y \\ v_x \\ v_y \end{bmatrix} x= ​xyvx​vy​​ ​
  • 观测空间为 z = [ x y ] z = \begin{bmatrix} x \\ y \end{bmatrix} z=[xy​]
  • 我们设定速度为 v = [ v x v y ] \mathbf{v}= \begin{bmatrix} v_x \\ v_y \end{bmatrix} v=[vx​vy​​]
  • 那么速度的导数就是 d v d t = ω [ 0 − 1 1 0 ] v \frac{d\mathbf{v}}{dt} = \omega \begin{bmatrix} 0 & -1 \\ 1 & 0 \end{bmatrix} \mathbf{v} dtdv​=ω[01​−10​]v
    • 因为在极短的时间步下 θ = ω Δ t ≈ 0 \theta = \omega \Delta t \approx 0 θ=ωΔt≈0，因此有 cos ⁡ ( θ ) ≈ 1 , sin ⁡ ( θ ) ≈ θ \cos(\theta) \approx 1,\quad \sin(\theta) \approx \theta cos(θ)≈1,sin(θ)≈θ
    • 那么旋转矩阵就是 R ( θ ) ≈ [ 1 − ω Δ t ω Δ t 1 ] R(\theta) \approx \begin{bmatrix} 1 & -\omega \Delta t \\ \omega \Delta t & 1 \end{bmatrix} R(θ)≈[1ωΔt​−ωΔt1​]
  • 也就是说整体系统函数可以写成： f ( x ) = [ x + v x Δ t y + v y Δ t v x − ω v y Δ t v y + ω v x Δ t ] f(x) = \begin{bmatrix} x + v_x \Delta t \\ y + v_y \Delta t \\ v_x - \omega v_y \Delta t \\ v_y + \omega v_x \Delta t \end{bmatrix} f(x)= ​x+vx​Δty+vy​Δtvx​−ωvy​Δtvy​+ωvx​Δt​ ​
  • 所以该模型的雅克比矩阵就是： F = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 − ω Δ t 0 0 ω Δ t 1 ] F = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & -\omega \Delta t \\ 0 & 0 & \omega \Delta t & 1 \end{bmatrix} F= ​1000​0100​Δt01ωΔt​0Δt−ωΔt1​ ​
class EKF:
    def __init__(self):
        self.dt = DT
        # 角速度
        self.omega = OMEGA
        # 状态 [x,y,vx,vy]
        self.x = np.zeros((4, 1))
        # 初始协方差矩阵
        self.P = np.eye(4) * 10
        # 过程噪声矩阵
        self.Q = np.eye(4) * 0.2
        # 观测噪声矩阵
        self.R = np.eye(2) * 3.0
        # 观测矩阵
        self.H = np.array([
            [1, 0, 0, 0],
            [0, 1, 0, 0]
        ])

    # 非线性模型（CT）
    def f(self, x):
        px, py, vx, vy = x.flatten()

        px_new = px + vx * self.dt
        py_new = py + vy * self.dt

        vx_new = vx - self.omega * vy * self.dt
        vy_new = vy + self.omega * vx * self.dt

        return np.array([[px_new], [py_new], [vx_new], [vy_new]])

    # 雅克布矩阵Jacobian
    def F_jacobian(self):
        dt = self.dt
        w = self.omega

        return np.array([
            [1, 0, dt, 0],
            [0, 1, 0, dt],
            [0, 0, 1, -w * dt],
            [0, 0, w * dt, 1]
        ])

    def predict(self):
        F = self.F_jacobian()
        # 非线性函数预测状态
        self.x = self.f(self.x)
        # 雅克比矩阵预测协方差
        self.P = F @ self.P @ F.T + self.Q

    def update(self, z):
        z = np.array(z).reshape(2, 1)
        # 计算残差
        y = z - self.H @ self.x
        # 计算观测协方差
        S = self.H @ self.P @ self.H.T + self.R
        # 计算卡尔曼增益
        K = self.P @ self.H.T @ np.linalg.inv(S)

        # 更新状态
        self.x = self.x + K @ y
        # 更新协方差
        self.P = (np.eye(4) - K @ self.H) @ self.P
2-4 完整代码：
import numpy as np
import matplotlib.pyplot as plt

DT = 10.0
OMEGA = 0.05


# =========================
# KF（CV模型）
# =========================
class KF:
    def __init__(self):
        self.dt = DT
        self.x = np.zeros((4, 1))

        self.F = np.array([
            [1, 0, self.dt, 0],
            [0, 1, 0, self.dt],
            [0, 0, 1, 0],
            [0, 0, 0, 1]
        ])

        self.H = np.array([
            [1, 0, 0, 0],
            [0, 1, 0, 0]
        ])

        self.P = np.eye(4) * 10
        self.Q = np.eye(4) * 0.2
        self.R = np.eye(2) * 3.0

    def predict(self):
        self.x = self.F @ self.x
        self.P = self.F @ self.P @ self.F.T + self.Q

    def update(self, z):
        z = np.array(z).reshape(2, 1)

        y = z - self.H @ self.x
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)

        self.x = self.x + K @ y
        self.P = (np.eye(4) - K @ self.H) @ self.P


# =========================
# EKF（CT模型）
# =========================
class EKF:
    def __init__(self):
        self.dt = DT
        # 角速度
        self.omega = OMEGA
        # 状态 [x,y,vx,vy]
        self.x = np.zeros((4, 1))
        # 初始协方差矩阵
        self.P = np.eye(4) * 10
        # 过程噪声矩阵
        self.Q = np.eye(4) * 0.2
        # 观测噪声矩阵
        self.R = np.eye(2) * 3.0
        # 观测矩阵
        self.H = np.array([
            [1, 0, 0, 0],
            [0, 1, 0, 0]
        ])

    # 非线性模型（CT）
    def f(self, x):
        px, py, vx, vy = x.flatten()

        px_new = px + vx * self.dt
        py_new = py + vy * self.dt

        vx_new = vx - self.omega * vy * self.dt
        vy_new = vy + self.omega * vx * self.dt

        return np.array([[px_new], [py_new], [vx_new], [vy_new]])

    # 雅克布矩阵Jacobian
    def F_jacobian(self):
        dt = self.dt
        w = self.omega

        return np.array([
            [1, 0, dt, 0],
            [0, 1, 0, dt],
            [0, 0, 1, -w * dt],
            [0, 0, w * dt, 1]
        ])

    def predict(self):
        F = self.F_jacobian()
        # 非线性函数预测状态
        self.x = self.f(self.x)
        # 雅克比矩阵预测协方差
        self.P = F @ self.P @ F.T + self.Q

    def update(self, z):
        z = np.array(z).reshape(2, 1)
        # 计算残差
        y = z - self.H @ self.x
        # 计算观测协方差
        S = self.H @ self.P @ self.H.T + self.R
        # 计算卡尔曼增益
        K = self.P @ self.H.T @ np.linalg.inv(S)

        # 更新状态
        self.x = self.x + K @ y
        # 更新协方差
        self.P = (np.eye(4) - K @ self.H) @ self.P


# =========================
# Ground Truth（圆周）
# =========================
def true_state(t):
    x = 50 * np.cos(t)
    y = 50 * np.sin(t)
    return np.array([x, y])




# =========================
# Simulation
# =========================
kf = KF()
ekf = EKF()

T = 400

kf_rmse = []
ekf_rmse = []

kf_pos = []
ekf_pos = []
true_pos = []

t = 0

for i in range(T):

    # 真实轨迹
    true = true_state(t)
    t += OMEGA * DT

    # 带噪声观测
    obs = true + np.random.randn(2) * 0.8

    # ================= KF =================
    kf.predict()
    kf.update(obs)
    kf_est = kf.x[:2].flatten()

    # ================= EKF =================
    ekf.predict()
    ekf.update(obs)
    ekf_est = ekf.x[:2].flatten()

    # ================= RMSE =================
    kf_rmse.append(np.linalg.norm(true - kf_est))
    ekf_rmse.append(np.linalg.norm(true - ekf_est))

    kf_pos.append(kf_est)
    ekf_pos.append(ekf_est)
    true_pos.append(true)


# =========================
# Plot
# =========================
kf_pos = np.array(kf_pos)
ekf_pos = np.array(ekf_pos)
true_pos = np.array(true_pos)

plt.figure(figsize=(12, 5))

# 轨迹
plt.subplot(1, 2, 1)
plt.plot(true_pos[:, 0], true_pos[:, 1], 'g', label="True")
plt.plot(kf_pos[:, 0], kf_pos[:, 1], 'r--', label="KF")
plt.plot(ekf_pos[:, 0], ekf_pos[:, 1], 'b-', label="EKF")
plt.legend()
plt.title("Trajectory Comparison")
plt.axis("equal")

# RMSE
plt.subplot(1, 2, 2)
plt.plot(kf_rmse, 'r', label="KF RMSE")
plt.plot(ekf_rmse, 'b', label="EKF RMSE")
plt.legend()
plt.title("RMSE Comparison")

plt.show()
  • 可以看到EKF的估计效果更好请添加图片描述
2-4 EKF 的局限性
  • 尽管EKF利用一阶泰勒展开将非线性转换为线性，但有明显问题：

EKF 只使用“一个点的导数信息（Jacobian）”，来近似整个非线性函数

  • 这样的估计会有很大的缺陷：
    • 高度非线性时容易发散
    • 雅可比计算复杂
    • 一阶近似误差不可控
    • 对初值敏感
  • 为了解决 EKF 的问题，人们提出了UKF（Unscented Kalman Filter），也就是不再线性化函数，而是直接“传播一组采样点”。

3 UKF 无迹卡尔曼滤波

3-1 核心思想
  • UKF（Unscented Kalman Filter）的核心思想非常直接：

不再线性化函数，直接用“多个代表点”去近似整个高斯分布

  • 也是就说我们需要构造一组有限的点，去替代高斯分布：
    • 能表示均值
    • 能表示协方差（椭圆形状）
3-2 Sigma Point采样点与Cholesky分解

  • 为此，我们设计中心点来表示均值 χ 0 = x ^ \chi_0=\hat{x} χ0​=x^

  • 紧接着我们需要一组点来表示协方差，我们搜先在“标准坐标系”里取一个对称超球/超方体上的点 x ^ ± n + λ \hat{x} \pm \sqrt{n+\lambda} x^±n+λ ​其中： n + λ \sqrt{n+\lambda} n+λ ​控制“采样半径”，越大点越分散，越小点越集中

  • 但是考虑到协方差其实是一个椭圆，因此我们需要借助 Cholesky分解将把标准球变成椭球

  • Cholesky分解 P = L L T P=LL^T P=LLT

    • L是把标准高斯分布的“等密度球面” → 协方差定义的椭球

  • 所以我们有： χ i = x ^ ± n + λ ⋅ L i = x ^ ± n + λ P \chi_i = \hat{x} \pm \sqrt{n+\lambda} \cdot L_i= \hat{x} \pm \sqrt{n+\lambda P} χi​=x^±n+λ ​⋅Li​=x^±n+λP ​

3-2 参数含义

  • 总结一下，对于状态维度为 n n n的系统，我们有 2 n + 1 2n+1 2n+1个采样点： χ 0 = x ^ χ i = x ^ + n + λ P i χ i + n = x ^ − n + λ P i \begin{align} \chi_0&=\hat{x}\\ \chi_i &= \hat{x} + \sqrt{n+\lambda P_i}\\ \chi_{i+n} &= \hat{x} - \sqrt{n+\lambda P_i} \end{align} χ0​χi​χi+n​​=x^=x^+n+λPi​ ​=x^−n+λPi​ ​​​

  • 为了重构原高斯分布的统计特性，我们要对这些点进行加权计算整个分布的均值和协方差，如下：

  • 均值： W 0 ( m ) = λ n + λ W i ( m ) = 1 2 ( n + λ ) \begin{align} W_0^{(m)} &= \frac{\lambda}{n+\lambda}\\ W_i^{(m)} &= \frac{1}{2(n+\lambda)} \end{align} W0(m)​Wi(m)​​=n+λλ​=2(n+λ)1​​​

  • 协方差： W 0 ( c ) = λ n + λ + ( 1 − α 2 + β ) W i ( c ) = 1 2 ( n + λ ) \begin{align} W_0^{(c)} &= \frac{\lambda}{n+\lambda} + (1 - \alpha^2 + \beta)\\ W_i^{(c)} &= \frac{1}{2(n+\lambda)} \end{align} W0(c)​Wi(c)​​=n+λλ​+(1−α2+β)=2(n+λ)1​​​
    λ = α 2 ( n + κ ) − n \lambda = \alpha^2 (n + \kappa)-n λ=α2(n+κ)−n

  • 其中：

    • λ \lambda λ：采样范围控制器系数，控制 sigma points 的“整体半径”
      • λ \lambda λ越大： sigma points 在椭球外面“扩散”
      • λ \lambda λ越小：sigma points 更贴近均值
    • α \alpha α：sigma points 的“扩散尺度”
      • α \alpha α越小，点靠近均值
      • α \alpha α越大，点离均值远（激进）
    • k k k：经验修正项，调整 λ 的偏移，让高维情况下数值更稳定
    • β \beta β：修正“分布的先验形状信息”，高斯分布推荐设置为2
3-3 公式推导
  • 那么我们正是开始
3-3-1 模型
  • 依旧是非线性系统起手：
    x k = f ( x k − 1 ) + w k , w k ∼ N ( 0 , Q ) z k = h ( x k ) + v k , v k ∼ N ( 0 , R ) ) \begin{align} x_k & = f(x_{k-1}) + w_k,\quad w_k \sim \mathcal{N}(0,Q) \\ z_k &= h(x_k) + v_k,\quad v_k \sim \mathcal{N}(0,R)) \end{align} xk​zk​​=f(xk−1​)+wk​,wk​∼N(0,Q)=h(xk​)+vk​,vk​∼N(0,R))​​
3-3-2 构造Sigma Point

χ 0 = x ^ χ i = x ^ + n + λ P i χ i + n = x ^ − n + λ P i \begin{align} \chi_0&=\hat{x}\\ \chi_i &= \hat{x} + \sqrt{n+\lambda P_i}\\ \chi_{i+n} &= \hat{x} - \sqrt{n+\lambda P_i} \end{align} χ0​χi​χi+n​​=x^=x^+n+λPi​ ​=x^−n+λPi​ ​​​

  • 其中： P = L L T P=LL^T P=LLT
  • 因此 χ i = x ^ ± n + λ   L i \chi_i = \hat{x} \pm \sqrt{n+\lambda}\, L_i χi​=x^±n+λ ​Li​
3-3-3 传播 Sigma Points（核心）
  • 状态传播 χ i − = f ( χ i ) \chi_i^- = f(\chi_i) χi−​=f(χi​)
  • 均值预测： x ^ − = ∑ i = 0 2 n W i ( m ) χ i − \hat{x}^- = \sum_{i=0}^{2n} W_i^{(m)} \chi_i^- x^−=i=0∑2n​Wi(m)​χi−​
  • 协方差预测： P − = ∑ i = 0 2 n W i ( c ) ( χ i − − x ^ − ) ( χ i − − x ^ − ) T + Q P^- = \sum_{i=0}^{2n} W_i^{(c)} (\chi_i^- - \hat{x}^-)(\chi_i^- - \hat{x}^-)^T + Q P−=i=0∑2n​Wi(c)​(χi−​−x^−)(χi−​−x^−)T+Q
3-3-4 传播观测预测（同样方式）
  • 观测Sigma变换： γ i = h ( χ i − ) \gamma_i = h(\chi_i^-) γi​=h(χi−​)
  • 观测均值： z ^ = ∑ W i ( m ) γ i \hat{z} = \sum W_i^{(m)} \gamma_i z^=∑Wi(m)​γi​
  • 观测协方差： P z z = ∑ W i ( c ) ( γ i − z ^ ) ( γ i − z ^ ) T + R P_{zz} = \sum W_i^{(c)} (\gamma_i - \hat{z})(\gamma_i - \hat{z})^T + R Pzz​=∑Wi(c)​(γi​−z^)(γi​−z^)T+R
  • 状态-观测协方差 P x z = ∑ W i ( c ) ( χ i − − x ^ − ) ( γ i − z ^ ) T P_{xz} = \sum W_i^{(c)} (\chi_i^- - \hat{x}^-)(\gamma_i - \hat{z})^T Pxz​=∑Wi(c)​(χi−​−x^−)(γi​−z^)T
3-3-5 卡尔曼增益

K = P x z P z z − 1 K = P_{xz} P_{zz}^{-1} K=Pxz​Pzz−1​

3-3-6 更新
  • 状态更新 x ^ = x ^ − + K ( z − z ^ ) \hat{x} = \hat{x}^- + K(z - \hat{z}) x^=x^−+K(z−z^)
  • 协方差更新 P = P − − K P z z K T P = P^- - K P_{zz} K^T P=P−−KPzz​KT

3-4 实现

# =========================
# state = [px, py, vx, vy, ax, ay]
# =========================
class UKF:
    def __init__(self, dt=0.1):
        # 状态维度
        self.n = 6
        # 观测维度
        self.m = 2
        self.dt = dt

        # 状态
        self.x = np.zeros((self.n, 1))
        # 初始协方差
        self.P = np.eye(self.n) * 1.0

        # 过程噪声矩阵
        self.Q = np.eye(self.n) * 0.1
        # 测量噪声矩阵
        self.R = np.eye(self.m) * 0.5

        # 阿尔法，扩散尺度
        self.alpha = 0.001
        # k，经验修正项
        self.kappa = 0
        # 修正“分布的先验形状信息”，高斯分布推荐2
        self.beta = 2

        # lambda
        self.lmbda = self.alpha**2 * (self.n + self.kappa) - self.n
        # 偏移量
        self.gamma = np.sqrt(self.n + self.lmbda)


        self.Wm = np.zeros(2*self.n + 1)
        self.Wc = np.zeros(2*self.n + 1)

        # 中心点加权
        self.Wm[0] = self.lmbda / (self.n + self.lmbda)
        self.Wc[0] = self.Wm[0] + (1 - self.alpha**2 + self.beta)

        # 加权均值与加权协方差
        for i in range(1, 2*self.n + 1):
            self.Wm[i] = 1 / (2*(self.n + self.lmbda))
            self.Wc[i] = self.Wm[i]

    # =========================
    # sigma points
    # =========================
    def sigma_points(self, x, P):
        sigma = np.zeros((2*self.n+1, self.n))
        # Cholesky分解
        U = np.linalg.cholesky(P + 1e-6*np.eye(self.n))

        sigma[0] = x.ravel()

        # 预测sigma points
        for i in range(self.n):
            sigma[i+1] = (x + self.gamma * U[:, i:i+1]).ravel()
            sigma[i+1+self.n] = (x - self.gamma * U[:, i:i+1]).ravel()

        return sigma

    # =========================
    # CA motion model
    # =========================
    def f(self, x):
        px, py, vx, vy, ax, ay = x
        dt = self.dt

        px = px + vx*dt + 0.5*ax*dt*dt
        py = py + vy*dt + 0.5*ay*dt*dt
        vx = vx + ax*dt
        vy = vy + ay*dt

        return np.array([px, py, vx, vy, ax, ay])

    # =========================
    # measurement model
    # =========================
    def h(self, x):
        return np.array([x[0], x[1]])

    # =========================
    # predict
    # =========================
    def predict(self):
        # 计算sigma points
        sigma = self.sigma_points(self.x, self.P)
        # 非线性传播，把每一个点都带入系统模型
        sigma_f = np.array([self.f(s) for s in sigma])

        # 计算均值
        x_pred = np.zeros((self.n, 1))
        for i in range(2*self.n+1):
            x_pred += self.Wm[i] * sigma_f[i].reshape(-1,1)

        # 计算协方差
        P_pred = np.zeros((self.n, self.n))
        for i in range(2*self.n+1):
            diff = sigma_f[i].reshape(-1,1) - x_pred
            P_pred += self.Wc[i] * diff @ diff.T

        # 加入过程噪声
        P_pred += self.Q

        self.X_sigma = sigma_f
        self.x = x_pred
        self.P = P_pred

    # =========================
    # update
    # =========================
    def update(self, z):
        z = z.reshape(-1,1)
        # sigma points映射到观测空间
        Z_sigma = np.array([self.h(s) for s in self.X_sigma])

        # 观测均值
        z_pred = np.zeros((self.m,1))
        for i in range(2*self.n+1):
            z_pred += self.Wm[i] * Z_sigma[i].reshape(-1,1)

        # 观测协方差 S
        S = np.zeros((self.m,self.m))
        # 
        Pxz = np.zeros((self.n,self.m))

        # 计算Pxz
        for i in range(2*self.n+1):
            dz = Z_sigma[i].reshape(-1,1) - z_pred
            dx = self.X_sigma[i].reshape(-1,1) - self.x

            S += self.Wc[i] * dz @ dz.T
            Pxz += self.Wc[i] * dx @ dz.T

        # 加测量噪声
        S += self.R

        # Kalman Gain
        K = Pxz @ np.linalg.inv(S)

        # 状态更新
        self.x = self.x + K @ (z - z_pred)
        # 协方差更新
        self.P = self.P - K @ S @ K.T
3-5 完整代码
import numpy as np
import matplotlib.pyplot as plt

# =========================
# state = [px, py, vx, vy, ax, ay]
# =========================
class UKF:
    def __init__(self, dt=0.1):
        # 状态维度
        self.n = 6
        # 观测维度
        self.m = 2
        self.dt = dt

        # 状态
        self.x = np.zeros((self.n, 1))
        # 初始协方差
        self.P = np.eye(self.n) * 1.0

        # 过程噪声矩阵
        self.Q = np.eye(self.n) * 0.1
        # 测量噪声矩阵
        self.R = np.eye(self.m) * 0.5

        # 阿尔法，扩散尺度
        self.alpha = 0.001
        # k，经验修正项
        self.kappa = 0
        # 修正“分布的先验形状信息”，高斯分布推荐2
        self.beta = 2

        # lambda
        self.lmbda = self.alpha**2 * (self.n + self.kappa) - self.n
        # 偏移量
        self.gamma = np.sqrt(self.n + self.lmbda)


        self.Wm = np.zeros(2*self.n + 1)
        self.Wc = np.zeros(2*self.n + 1)

        # 中心点加权
        self.Wm[0] = self.lmbda / (self.n + self.lmbda)
        self.Wc[0] = self.Wm[0] + (1 - self.alpha**2 + self.beta)

        # 加权均值与加权协方差
        for i in range(1, 2*self.n + 1):
            self.Wm[i] = 1 / (2*(self.n + self.lmbda))
            self.Wc[i] = self.Wm[i]

    # =========================
    # sigma points
    # =========================
    def sigma_points(self, x, P):
        sigma = np.zeros((2*self.n+1, self.n))
        # Cholesky分解
        U = np.linalg.cholesky(P + 1e-6*np.eye(self.n))

        sigma[0] = x.ravel()

        # 预测sigma points
        for i in range(self.n):
            sigma[i+1] = (x + self.gamma * U[:, i:i+1]).ravel()
            sigma[i+1+self.n] = (x - self.gamma * U[:, i:i+1]).ravel()

        return sigma

    # =========================
    # CA motion model
    # =========================
    def f(self, x):
        px, py, vx, vy, ax, ay = x
        dt = self.dt

        px = px + vx*dt + 0.5*ax*dt*dt
        py = py + vy*dt + 0.5*ay*dt*dt
        vx = vx + ax*dt
        vy = vy + ay*dt

        return np.array([px, py, vx, vy, ax, ay])

    # =========================
    # measurement model
    # =========================
    def h(self, x):
        return np.array([x[0], x[1]])

    # =========================
    # predict
    # =========================
    def predict(self):
        # 计算sigma points
        sigma = self.sigma_points(self.x, self.P)
        # 非线性传播，把每一个点都带入系统模型
        sigma_f = np.array([self.f(s) for s in sigma])

        # 计算均值
        x_pred = np.zeros((self.n, 1))
        for i in range(2*self.n+1):
            x_pred += self.Wm[i] * sigma_f[i].reshape(-1,1)

        # 计算协方差
        P_pred = np.zeros((self.n, self.n))
        for i in range(2*self.n+1):
            diff = sigma_f[i].reshape(-1,1) - x_pred
            P_pred += self.Wc[i] * diff @ diff.T

        # 加入过程噪声
        P_pred += self.Q

        self.X_sigma = sigma_f
        self.x = x_pred
        self.P = P_pred

    # =========================
    # update
    # =========================
    def update(self, z):
        z = z.reshape(-1,1)
        # sigma points映射到观测空间
        Z_sigma = np.array([self.h(s) for s in self.X_sigma])

        # 观测均值
        z_pred = np.zeros((self.m,1))
        for i in range(2*self.n+1):
            z_pred += self.Wm[i] * Z_sigma[i].reshape(-1,1)

        # 观测协方差 S
        S = np.zeros((self.m,self.m))
        # 
        Pxz = np.zeros((self.n,self.m))

        # 计算Pxz
        for i in range(2*self.n+1):
            dz = Z_sigma[i].reshape(-1,1) - z_pred
            dx = self.X_sigma[i].reshape(-1,1) - self.x

            S += self.Wc[i] * dz @ dz.T
            Pxz += self.Wc[i] * dx @ dz.T

        # 加测量噪声
        S += self.R

        # Kalman Gain
        K = Pxz @ np.linalg.inv(S)

        # 状态更新
        self.x = self.x + K @ (z - z_pred)
        # 协方差更新
        self.P = self.P - K @ S @ K.T


# =========================
# CA ground truth model
# =========================
def true_state(t):
    x0, y0 = 0.0, 0.0
    vx0, vy0 = 1.0, 0.5
    ax, ay = 0.05, -0.02

    x = x0 + vx0*t + 0.5*ax*t*t
    y = y0 + vy0*t + 0.5*ay*t*t

    vx = vx0 + ax*t
    vy = vy0 + ay*t

    return np.array([x, y, vx, vy])
# =========================
# run simulation
# =========================
dt = 0.1
T = 200

ukf = UKF(dt)

ukf.x = np.array([[50],[0],[0],[5],[0],[0]])

rmse = []
for k in range(T):
    t = k*dt

    true = true_state(t)

    # 只观测位置
    z = true[:2] + np.random.randn(2)*1.0

    ukf.predict()
    ukf.update(z)

    err = np.linalg.norm(ukf.x[:2].ravel() - true[:2])
    rmse.append(err)

# =========================
# plot RMSE
# =========================
plt.plot(rmse, 'b', label="UKF RMSE")
plt.xlabel("step")
plt.ylabel("RMSE")
plt.legend()
plt.grid()
plt.show()

请添加图片描述

3-6 UKF的局限性
  • UKF（Unscented Kalman Filter）通过 sigma points 在非线性系统中传播均值和协方差，相比 EKF 不需要显式求雅可比矩阵，在很多工程问题中表现稳定、实现简单。但它仍然基于一个核心假设：

系统状态分布始终可以用“单个高斯分布”近似表示

  • 当系统出现以下情况时，UKF开始失效：
    • 多模态分布（multi-modal）
    • 强非线性
    • 非高斯噪声
    • 遮挡 / 数据关联不确定
  • 这时需要更通用的方法：Particle Filter（粒子滤波）

4 粒子滤波

4-1 蒙特卡洛采样
  • 在贝叶斯滤波中，我们反复遇到一个核心形式： p ( x k ∣ z 1 : k ) ∝ p ( z k ∣ x k )   p ( x k ∣ z 1 : k − 1 ) p(x_k | z_{1:k}) \propto p(z_k | x_k)\, p(x_k | z_{1:k-1}) p(xk​∣z1:k​)∝p(zk​∣xk​)p(xk​∣z1:k−1​)
  • 但真正困难的是预测步骤中的积分： p ( x k ∣ z 1 : k − 1 ) = ∫ p ( x k ∣ x k − 1 )   p ( x k − 1 ∣ z 1 : k − 1 )   d x k − 1 p(x_k | z_{1:k-1}) = \int p(x_k | x_{k-1})\, p(x_{k-1} | z_{1:k-1}) \, dx_{k-1} p(xk​∣z1:k−1​)=∫p(xk​∣xk−1​)p(xk−1​∣z1:k−1​)dxk−1​
    这个积分在非线性系统中：
    • 没有解析解
    • 也无法用简单高斯传播表示
    • EKF / UKF 本质都是在“近似这个积分”
  • 蒙特卡洛方法的核心就一句话：

用“采样 + 统计平均”代替积分

  • 对于期望形式积分： E [ f ( x ) ] = ∫ f ( x ) p ( x )   d x \mathbb{E}[f(x)] = \int f(x)p(x)\,dx E[f(x)]=∫f(x)p(x)dx
  • 若从分布 p ( x ) p(x) p(x) 中独立同分布采样 x ( i ) ∼ p ( x ) x^{(i)} \sim p(x) x(i)∼p(x)，则有近似： E [ f ( x ) ] ≈ 1 N ∑ i = 1 N f ( x ( i ) ) \mathbb{E}[f(x)] \approx \frac{1}{N} \sum_{i=1}^{N} f(x^{(i)}) E[f(x)]≈N1​i=1∑N​f(x(i))
  • 根据大数定律，当 N → ∞ N \to \infty N→∞ 时，该估计在概率意义下收敛于真实期望。
  • 其中：
    • x ( i ) ∼ p ( x ) x^{(i)} \sim p(x) x(i)∼p(x)（从分布中随机采样）
    • N 越大，越接近真实积分
4-2 粒子滤波的三步流程
  • Particle Filter 每一轮只做三件事：
4-2-1 预测：粒子传播
  • 对每个粒子进行状态传播： x k ( i ) ∼ p ( x k ∣ x k − 1 ( i ) ) x_k^{(i)} \sim p(x_k \mid x_{k-1}^{(i)}) xk(i)​∼p(xk​∣xk−1(i)​)
  • 在工程实现中通常写成： x k ( i ) = f ( x k − 1 ( i ) ) + w k x_k^{(i)} = f(x_{k-1}^{(i)}) + w_k xk(i)​=f(xk−1(i)​)+wk​
  • 意思就是：
    • 每个粒子都按照动力学模型“走一步”
    • 同时加入随机噪声模拟系统不确定性
4-2-2 更新权重
  • 利用观测 z k z_k zk​ 评估每个粒子的“可信程度”： w k ( i ) ∝ p ( z k ∣ x k ( i ) ) w_k^{(i)} \propto p(z_k \mid x_k^{(i)}) wk(i)​∝p(zk​∣xk(i)​)
  • 如果观测噪声为高斯分布，则常用形式为： w k ( i ) = exp ⁡ ( − 1 2 ( z k − h ( x k ( i ) ) ) T R − 1 ( z k − h ( x k ( i ) ) ) ) w_k^{(i)} = \exp\left(-\frac{1}{2}(z_k - h(x_k^{(i)}))^T R^{-1} (z_k - h(x_k^{(i)}))\right) wk(i)​=exp(−21​(zk​−h(xk(i)​))TR−1(zk​−h(xk(i)​)))
  • 然后进行归一化： w k ( i ) = w k ( i ) ∑ j = 1 N w k ( j ) w_k^{(i)} = \frac{w_k^{(i)}}{\sum_{j=1}^{N} w_k^{(j)}} wk(i)​=∑j=1N​wk(j)​wk(i)​​
  • 也就是：

“越符合观测的粒子，活得越久；不符合的，逐渐被淘汰”

4-2-2 重采样（Resampling）
  • 为了避免“权重退化问题”（weight degeneracy），需要重新采样： { x k ( i ) } ∼ Multinomial ( w k ( i ) ) \{x_k^{(i)}\} \sim \text{Multinomial}(w_k^{(i)}) {xk(i)​}∼Multinomial(wk(i)​)
  • 作用是：
    • 高权重粒子被复制
    • 低权重粒子被丢弃
  • 如果不重采样，会发生一个灾难：

大部分粒子权重变成 0，只有少数粒子存活（粒子退化）

4-4 核心实现

# =========================
# Particle Filter
# =========================
class ParticleFilter:
    def __init__(self, N=300):
        # 粒子数量
        self.N = N

        # 粒子: [x, y, vx, vy]
        self.particles = np.zeros((N, 4))

        # 权重
        self.weights = np.ones(N) / N

    # =========================
    # 非线性运动模型（随机加速度）
    # =========================
    def motion(self, x):
        dt = 0.1

        ax, ay = np.random.randn(2) * 0.2

        x[0] += x[2] * dt + 0.5 * ax * dt**2
        x[1] += x[3] * dt + 0.5 * ay * dt**2
        x[2] += ax * dt
        x[3] += ay * dt

        return x

    # =========================
    # 非线性观测模型 (range + bearing)
    # =========================
    def observe(self, x):
        px, py = x[0], x[1]
        r = np.sqrt(px**2 + py**2)
        theta = np.arctan2(py, px)
        return np.array([r, theta])

    # =========================
    # 预测
    # =========================
    def predict(self):
        # 粒子传播，对每一个粒子进行模型计算
        for i in range(self.N):
            noise = np.random.randn(4) * 0.1
            self.particles[i] = self.motion(self.particles[i]) + noise

    # =========================
    # 更新权重
    # =========================
    def update(self, z):
        for i in range(self.N):
            z_hat = self.observe(self.particles[i])
            # 计算误差
            diff = z - z_hat

            # 观测噪声
            R = np.array([0.3, 0.05])

            # 计算权重
            self.weights[i] = np.exp(
                -0.5 * np.sum((diff / R) ** 2)
            )

        # 归一化
        self.weights += 1e-12
        self.weights /= np.sum(self.weights)

    # =========================
    # 重采样
    # =========================
    def resample(self):
        idx = np.random.choice(self.N, self.N, p=self.weights)
        self.particles = self.particles[idx]
        self.weights[:] = 1.0 / self.N

    # =========================
    # 状态估计
    # =========================
    def estimate(self):
        return np.average(self.particles, weights=self.weights, axis=0)
4-5 完整代码
import numpy as np
import matplotlib.pyplot as plt


# =========================
# Particle Filter
# =========================
class ParticleFilter:
    def __init__(self, N=300):
        # 粒子数量
        self.N = N

        # 粒子: [x, y, vx, vy]
        self.particles = np.zeros((N, 4))

        # 权重
        self.weights = np.ones(N) / N

    # =========================
    # 非线性运动模型（随机加速度）
    # =========================
    def motion(self, x):
        dt = 0.1

        ax, ay = np.random.randn(2) * 0.2

        x[0] += x[2] * dt + 0.5 * ax * dt**2
        x[1] += x[3] * dt + 0.5 * ay * dt**2
        x[2] += ax * dt
        x[3] += ay * dt

        return x

    # =========================
    # 非线性观测模型 (range + bearing)
    # =========================
    def observe(self, x):
        px, py = x[0], x[1]
        r = np.sqrt(px**2 + py**2)
        theta = np.arctan2(py, px)
        return np.array([r, theta])

    # =========================
    # 预测
    # =========================
    def predict(self):
        # 粒子传播，对每一个粒子进行模型计算
        for i in range(self.N):
            noise = np.random.randn(4) * 0.1
            self.particles[i] = self.motion(self.particles[i]) + noise

    # =========================
    # 更新权重
    # =========================
    def update(self, z):
        for i in range(self.N):
            z_hat = self.observe(self.particles[i])
            # 计算误差
            diff = z - z_hat

            # 观测噪声
            R = np.array([0.3, 0.05])

            # 计算权重
            self.weights[i] = np.exp(
                -0.5 * np.sum((diff / R) ** 2)
            )

        # 归一化
        self.weights += 1e-12
        self.weights /= np.sum(self.weights)

    # =========================
    # 重采样
    # =========================
    def resample(self):
        idx = np.random.choice(self.N, self.N, p=self.weights)
        self.particles = self.particles[idx]
        self.weights[:] = 1.0 / self.N

    # =========================
    # 状态估计
    # =========================
    def estimate(self):
        return np.average(self.particles, weights=self.weights, axis=0)


# =========================
# Ground Truth (circle motion)
# =========================
def true_motion(t):
    r = 10
    omega = 0.1
    x = r * np.cos(omega * t)
    y = r * np.sin(omega * t)
    return np.array([x, y])


# =========================
# True observation
# =========================
def observe_true(x):
    px, py = x
    r = np.sqrt(px**2 + py**2)
    theta = np.arctan2(py, px)
    return np.array([r, theta])


# =========================
# Simulation
# =========================
pf = ParticleFilter(N=300)

T = 200
dt = 0.1

true_traj = []
est_traj = []
rmse_list = []

for k in range(T):

    t = k * dt

    # ===== true state =====
    true_x = true_motion(t)
    true_traj.append(true_x.copy())

    # ===== nonlinear noisy observation =====
    z_true = observe_true(true_x)
    z = z_true + np.random.randn(2) * np.array([0.3, 0.05])

    # ===== PF =====
    pf.predict()
    pf.update(z)
    pf.resample()

    est = pf.estimate()[:2]
    est_traj.append(est)

    # ===== RMSE =====
    rmse = np.linalg.norm(est - true_x)
    rmse_list.append(rmse)


# =========================
# Convert
# =========================
true_traj = np.array(true_traj)
est_traj = np.array(est_traj)

# =========================
# Plot
# =========================
plt.figure(figsize=(12, 5))

# trajectory
plt.subplot(1, 2, 1)
plt.plot(true_traj[:, 0], true_traj[:, 1], label="True (circle)")
plt.plot(est_traj[:, 0], est_traj[:, 1], '--', label="Particle Filter")
plt.legend()
plt.title("Nonlinear Tracking (PF)")
plt.axis("equal")
plt.grid()

# RMSE
plt.subplot(1, 2, 2)
plt.plot(rmse_list, label="RMSE")
plt.legend()
plt.title("RMSE Curve")
plt.grid()

plt.show()

请添加图片描述

EKF UKF PF对比
方法 近似方式 是否线性化 分布假设 计算复杂度 鲁棒性
EKF 一阶泰勒展开 是 高斯 最低 弱
UKF Sigma points传播 否 高斯 中等 较强
PF Monte Carlo采样 否 无限制 最高 最强

总结

  • 本期我们探讨了非线性空间下状态估计的方法，包括：扩展卡尔曼滤波、无迹卡尔曼滤和粒子滤波的原理推导和基础python实现。
  • 如有错误，欢迎指出！
  • 感谢观看！
# 线性代数 # 矩阵 # python
Logo
AI Agent技术社区

Agent 垂直技术社区，欢迎活跃、内容共建。

更多推荐

cover

深入理解 AI Agent · AGENT #02：Agent 的四大核心机制，拆开来看每个都在干什么

avatar AI Agent技术社区

MCP协议开发实战：从零搭建AI Agent工具链

本文介绍了MCP协议在AI工具链中的应用与实践。MCP协议旨在解决异构工具集成难题，为AI模型提供标准化、安全的工具调用接口。文章详细解析了MCP的核心架构，包括Server、Client、Transport等组件，以及通信流程与安全模型。通过实战示例，指导读者开发计算器和文件系统工具Server，并集成到Claude等AI客户端中。进阶部分展示了如何构建复杂工具链，包括API集成、数据库查询和多

avatar AI Agent技术社区
cover

AI Tool 设计：粒度、参数与错误恢复怎么做

avatar AI Agent技术社区