无迹卡尔曼滤波算法推导（上）：状态预测步骤详解

好，咱们今天来啃UKF最核心的一块——状态预测。说实话，我刚接触UKF那会儿，看到一堆sigma点和权重计算，头都大了。后来在项目里调了两个月电池SOC估计，才慢慢摸到门道。今天我就把这块掰开揉碎了讲给你听。

为什么需要无迹变换？

先想一个问题：电池模型是非线性的，对吧？EKF（扩展卡尔曼滤波）的做法是把非线性函数泰勒展开，只保留一阶项。说白了就是“近似”。但近似就有误差，尤其在电池这种强非线性系统里，误差会累积。

UKF的思路完全不同——它不近似函数，而是近似概率分布。怎么近似？用一组精心挑选的采样点（sigma点）来代表状态分布。这些点经过非线性函数后，再加权组合，就能得到预测后的均值和协方差。

核心思想：对非线性函数的概率分布进行近似，比直接近似函数本身更准确。

我在做磷酸铁锂电池SOC估计时遇到过这种情况：EKF在低SOC区域（<20%）误差能到8%，换成UKF后直接降到3%以内。原因就是磷酸铁锂的OCV-SOC曲线在两端特别平缓，非线性度极高，EKF的线性近似根本扛不住。

UKF状态预测的五个步骤

状态预测说白了就是“根据当前状态，猜下一步状态”。UKF分五步走，我一个个说。

第一步：计算sigma点

假设当前时刻k-1，我们有状态估计值 x̂_{k-1} 和协方差矩阵 P_{k-1}。现在要生成2n+1个sigma点（n是状态维度）。

公式长这样：

χ₀ = x̂
χᵢ = x̂ + (√((n+λ)P))ᵢ,  i = 1,...,n
χᵢ = x̂ - (√((n+λ)P))ᵢ₋ₙ,  i = n+1,...,2n

这里λ = α²(n+κ) - n，是个缩放参数。α控制sigma点的散布范围，一般取0.001到1之间。κ是次级缩放参数，通常设0或3-n。

我的经验：α取0.01左右效果不错。太小了sigma点太集中，非线性信息抓不住；太大了又可能采样到不合理区域。我在电池项目里试过α=0.5，结果协方差矩阵老是非正定，后来调回0.01就稳了。

你可能会问：“开根号那个怎么算？”嗯，这是对协方差矩阵做Cholesky分解。Python里用np.linalg.cholesky就行。

第二步：计算sigma点的权重

每个sigma点都有对应的权重，分均值权重和协方差权重两种：

W₀ᵐ = λ / (n + λ)          # 均值权重（第0个点）
W₀ᶜ = λ / (n + λ) + (1 - α² + β)  # 协方差权重（第0个点）
Wᵢᵐ = Wᵢᶜ = 1 / (2(n + λ))       # 其他点的权重，i=1,...,2n

β是状态分布的先验知识参数。高斯分布时β=2最优，这是我试过多次验证的。

参数 典型取值 作用
α 0.01 控制sigma点散布范围
β 2（高斯分布） 融入先验分布信息
κ 0 或 3-n 次级缩放参数

注意：权重之和必须等于1。我曾经因为手算时四舍五入，导致权重和不等于1，结果滤波器直接发散。检查权重和是个好习惯。

第三步：将sigma点通过状态方程

这一步最直接——把每个sigma点代入非线性状态方程f(·)：

χᵢ⁻ = f(χᵢ, u_{k-1})

对于电池模型，状态方程通常长这样：

# 电池状态方程示例（离散时间）
def state_equation(x, u, dt):
    """
    x: [SOC, V1, V2]  # 状态向量
    u: [I]             # 输入：电流
    """
    SOC, V1, V2 = x
    I = u[0]
    
    # SOC更新（安时积分）
    SOC_next = SOC - (I * dt) / Q_nominal
    
    # RC网络电压更新
    V1_next = V1 * exp(-dt/(R1*C1)) + I * R1 * (1 - exp(-dt/(R1*C1)))
    V2_next = V2 * exp(-dt/(R2*C2)) + I * R2 * (1 - exp(-dt/(R2*C2)))
    
    return np.array([SOC_next, V1_next, V2_next])

每个sigma点都过一遍这个函数，得到一组新的点χᵢ⁻。注意上标“-”表示预测（先验）值。

第四步：计算预测状态均值

把变换后的sigma点加权求和：

x̂ₖ⁻ = Σᵢ Wᵢᵐ · χᵢ⁻

说白了就是加权平均。权重Wᵢᵐ就是第二步算出来的均值权重。

这里有个细节——如果状态变量有物理约束（比如SOC必须在0~1之间），加权平均后可能会越界。我一般会在这一步之后加个裁剪：

x̂ₖ⁻[0] = np.clip(x̂ₖ⁻[0], 0, 1)  # 保证SOC在合理范围

第五步：计算预测协方差

最后一步，计算预测状态的不确定性：

Pₖ⁻ = Σᵢ Wᵢᶜ · (χᵢ⁻ - x̂ₖ⁻)(χᵢ⁻ - x̂ₖ⁻)ᵀ + Q

Q是过程噪声协方差矩阵。它代表你对模型本身的信任程度。Q越大，说明你觉得模型误差大，会更相信测量值。

调参心得：Q矩阵的对角线元素我一般设成状态量方差的1%~10%。比如SOC的方差如果是0.01²，那Q[0,0]就设1e-6左右。太小了滤波器收敛慢，太大了又容易震荡。

我曾经在一个项目中把Q设得特别小，结果滤波器对模型过度自信，测量更新基本不起作用，SOC估计值漂到天上去都不知道。后来花了三天才排查出来——就是Q太小惹的祸。

完整的状态预测代码

把上面五步串起来，就是完整的UKF状态预测函数：

def ukf_predict(x, P, f, Q, u, dt, alpha=0.01, beta=2, kappa=0):
    n = len(x)
    lam = alpha**2 * (n + kappa) - n
    
    # 1. 生成sigma点
    sqrt_P = np.linalg.cholesky((n + lam) * P)
    sigma_points = np.zeros((2*n + 1, n))
    sigma_points[0] = x
    for i in range(n):
        sigma_points[i+1] = x + sqrt_P[:, i]
        sigma_points[n+i+1] = x - sqrt_P[:, i]
    
    # 2. 计算权重
    W_m = np.full(2*n + 1, 1 / (2*(n + lam)))
    W_c = np.full(2*n + 1, 1 / (2*(n + lam)))
    W_m[0] = lam / (n + lam)
    W_c[0] = lam / (n + lam) + (1 - alpha**2 + beta)
    
    # 3. sigma点通过状态方程
    sigma_pred = np.array([f(sp, u, dt) for sp in sigma_points])
    
    # 4. 预测均值
    x_pred = np.dot(W_m, sigma_pred)
    
    # 5. 预测协方差
    diff = sigma_pred - x_pred
    P_pred = np.dot(W_c * diff.T, diff) + Q
    
    return x_pred, P_pred

这段代码我实际跑过很多次，用在18650电芯的SOC估计上，效果很稳。你直接拿去用，注意根据你的电池模型改一下state_equation函数就行。

一张图看懂UKF状态预测

下面这张SVG图把整个流程串起来了，建议你保存下来对照着看：

UKF状态预测流程 输入 x̂ₖ₋₁, Pₖ₋₁, uₖ₋₁, Q 步骤1：生成sigma点 χ₀ = x̂, χᵢ = x̂ ± √((n+λ)P) 步骤2：计算权重 Wᵐ, Wᶜ (均值权重与协方差权重) 步骤3：sigma点通过状态方程 χᵢ⁻ = f(χᵢ, uₖ₋₁) 每个点单独传播 步骤4：预测状态均值 x̂ₖ⁻ = Σ Wᵢᵐ · χᵢ⁻ 步骤5：预测协方差 Pₖ⁻ = Σ Wᵢᶜ · (χᵢ⁻-x̂ₖ⁻)² + Q 输出：x̂ₖ⁻, Pₖ⁻

从这张图能清楚看到：输入是上一时刻的状态和协方差，经过五步处理，输出预测后的状态和协方差。这就是UKF状态预测的全部秘密。

几个容易踩的坑

最后分享几个我实际踩过的坑：

  • Cholesky分解失败：协方差矩阵必须正定。如果出现非正定，检查一下是不是数值精度问题，或者加个很小的单位矩阵P + 1e-6*I。
  • sigma点越界：电池SOC、电压都有物理范围。sigma点生成后如果超出合理区间，可以考虑用反射法或裁剪法处理。
  • 权重精度：Python浮点数精度足够，但如果你用C/C++实现，注意float和double的区别。

好了，UKF状态预测这部分就讲到这里。你把这五步吃透了，后面测量更新那部分就顺理成章了。下一节咱们接着聊UKF的测量更新——说白了就是怎么用实际测量值来修正预测结果。

公众号:蓝海资料掘金营，微信deep3321

← 上一章 📖 返回目录 下一章 →