原理与实现：非线性状态估计的优雅解法)
1. 从卡尔曼到无迹为什么我们需要UKF如果你做过机器人定位、无人机导航或者任何涉及传感器融合的项目卡尔曼滤波Kalman Filter, KF这个名字你一定不陌生。它被誉为“最优估计器”在状态估计领域有着近乎神话般的地位。但当你真正动手把KF应用到实际项目中尤其是面对那些非线性系统时一个巨大的拦路虎就出现了雅可比矩阵。标准的卡尔曼滤波我们称之为扩展卡尔曼滤波EKF处理非线性问题的核心思路是“线性化”。它要求你对系统的状态转移方程和观测方程进行泰勒展开并计算其一阶导数雅可比矩阵。这个要求在实际工程中常常让人头疼。首先很多系统的模型本身就复杂求导过程繁琐且容易出错。其次对于高度非线性的系统一阶近似带来的误差可能非常大导致滤波结果发散也就是我们常说的“滤波器跑飞了”。最后有些系统甚至没有解析的导数形式比如一些黑盒模型或者查表模型EKF就完全无能为力了。那么有没有一种方法既能处理非线性问题又不用求那令人头疼的雅可比矩阵呢这就是无迹卡尔曼滤波Unscented Kalman Filter, UKF诞生的背景。我第一次接触UKF是在做一个四旋翼无人机的姿态估计项目上当时EKF因为模型线性化误差导致姿态角在快速机动时估计不准尝试UKF后稳定性和精度都有了肉眼可见的提升。UKF的核心思想非常巧妙它不再对非线性函数进行近似而是对状态的概率分布进行近似。具体来说它采用了一种叫做“无迹变换”Unscented Transform, UT的技术。UT的思想是与其去近似一个复杂的非线性函数不如精心挑选一组有代表性的样本点在UKF中称为Sigma点让这些点去“感受”非线性变换。将这些Sigma点通过真实的非线性函数传播后再对变换后的点集进行统计计算均值和协方差就能得到变换后状态分布的近似。这个过程完全避开了求导是一种“用采样代替求导”的思路。简单来说EKF是“我猜函数大概长这样线性”而UKF是“我找几个点去实际走一遍看看出来是什么样”。后者在处理强非线性时显然更可靠。接下来我们就深入UKF的数学原理和代码实现看看这套“无迹”的魔法是如何运作的。2. 无迹变换UT的数学直觉如何用几个点“代表”一个分布理解UKF必须先吃透无迹变换。这是整个算法的基石。我们从一个最简单的一维例子开始建立直观感受。假设我们有一个随机变量x它服从正态分布均值是m方差是P。现在我们有一个非线性函数y f(x)比如y sin(x)或者y x^2。我们想知道经过这个非线性变换后y的均值m_y和方差P_y是多少EKF的做法是在x m这一点对f(x)进行一阶泰勒展开f(x) ≈ f(m) f(m)*(x-m)。然后利用线性变换下均值和协方差的传播公式来计算。这相当于用一条直线切线来近似整个非线性函数。UT的做法则截然不同。它说我们选几个有代表性的x值Sigma点把它们代入真实的f(x)得到对应的y值然后对这些y值进行加权平均来估计m_y和P_y。关键在于如何选择这几个点以及如何给它们分配权重才能最“忠实”地反映原始分布x ~ N(m, P)的特性UKF采用了一套确定性的采样策略。对于一个n维的状态向量它会选取2n1个Sigma点。这些点不是随机采的而是根据状态的均值和协方差矩阵“计算”出来的。它们被设计成能够精确捕获输入分布的前两阶矩均值和协方差并且在经过非线性变换后对输出分布的前两阶矩的估计精度也能达到二阶以上而EKF只是一阶精度。具体的选择公式如下首先计算矩阵平方根。我们需要找到一个矩阵S使得P S * S^T。这通常通过对协方差矩阵P进行乔列斯基分解Cholesky decomposition来实现。S可以理解为标准差矩阵。然后围绕均值m对称地选取2n1个点X[0] m X[i] m sqrt(nλ) * S[:, i-1] for i 1,...,n X[in] m - sqrt(nλ) * S[:, i-1] for i 1,...,n这里的λ是一个缩放参数λ α²(nκ) - n。α控制Sigma点的散布范围通常取一个很小的正数如1e-3κ是一个次要缩放参数通常设为0或3-n。sqrt(nλ)就是缩放因子。每个Sigma点都对应两个权重一个用于计算均值W_m一个用于计算协方差W_c。通常第0个点均值点的权重最大其他对称点的权重相同。权重公式中包含了λ并且可以引入另一个参数β通常为2用于融合分布的高阶矩信息对于高斯分布是最优的。这个过程听起来有点抽象但你可以这样想象状态x的分布像一个n维的椭圆由协方差矩阵P描述。均值m是椭圆的中心。我们沿着这个椭圆的每一个主轴方向由S矩阵的列向量指示在正反两个方向上各走一段与轴长度和λ相关的距离得到一个点。这些点加上中心点就构成了能代表这个椭圆形状和位置的“骨架”点集。用这些点去通过非线性函数就好比让这个椭圆的“骨架”变形我们再根据变形后的骨架点重新拟合出一个新的椭圆输出分布的均值和协方差。与蒙特卡洛方法需要成千上万次随机采样相比UT只用2n1个点就达到了相当好的近似效果计算效率极高。这就是UKF“优雅”的地方。3. UKF算法流程分步拆解预测与更新的双步舞掌握了无迹变换UKF的整个算法流程就清晰了。和所有卡尔曼滤波家族成员一样UKF也遵循“预测-更新”的经典框架。下面我们结合公式和代码逻辑一步步拆解。我们假设系统模型如下状态方程x_k f(x_{k-1}, u_{k-1}) q_{k-1}其中f是非线性状态转移函数q是过程噪声协方差为Q。观测方程z_k h(x_k) r_k其中h是非线性观测函数r是观测噪声协方差为R。3.1 初始化首先我们需要对状态和协方差进行初始化。这通常基于系统的先验知识。# 假设状态维度为 n x np.array([...]) # n维向量初始状态估计 P np.eye(n) * 100 # n x n矩阵初始估计协方差通常设为一个较大的值表示不确定性大 Q np.diag([...]) # 过程噪声协方差矩阵需要根据系统特性 tuning R np.diag([...]) # 观测噪声协方差矩阵需要根据传感器特性 tuning这里的P初始值不能设为全零否则协方差矩阵会无法更新。Q和R是滤波器最重要的调参对象Q反映了你对模型信任度R反映了你对传感器信任度。3.2 预测步时间更新预测步的目标是利用系统模型将上一时刻的状态估计向前推演到当前时刻。步骤1计算Sigma点根据k-1时刻的后验估计x_{k-1|k-1}和协方差P_{k-1|k-1}利用上一节介绍的UT公式生成2n1个Sigma点X_{k-1}。def compute_sigma_points(x, P, alpha1e-3, beta2, kappa0): n len(x) lambda_ alpha**2 * (n kappa) - n # 计算矩阵平方根 (n x n) S np.linalg.cholesky((n lambda_) * P) # 注意这里乘了 (nλ) sigma_points np.zeros((2*n1, n)) sigma_points[0] x for i in range(n): sigma_points[i1] x S[i] sigma_points[ni1] x - S[i] # 计算权重 W_m np.zeros(2*n1) # 均值权重 W_c np.zeros(2*n1) # 协方差权重 W_m[0] lambda_ / (n lambda_) W_c[0] W_m[0] (1 - alpha**2 beta) for i in range(1, 2*n1): W_m[i] 1 / (2*(n lambda_)) W_c[i] W_m[i] return sigma_points, W_m, W_c注意实际中乔列斯基分解可能因为P非正定而失败。工业级代码会包含稳健的矩阵平方根计算比如使用scipy.linalg.sqrtm或添加一个微小的单位矩阵确保正定性。步骤2Sigma点通过状态转移函数将每一个Sigma点X_{k-1}^{(i)}通过非线性状态方程f进行传播得到预测的Sigma点X_{k|k-1}^{(i)}。# 假设有状态转移函数 f(x, u) predicted_sigma_points np.zeros_like(sigma_points) for i in range(2*n1): predicted_sigma_points[i] f(sigma_points[i], u) # u 是控制输入这一步是UKF与EKF的核心区别之一EKF只将均值点x通过f传播并用雅可比矩阵近似协方差的传播。而UKF让所有代表分布形状的Sigma点都去“经历”真实的非线性变换。步骤3计算预测状态均值与协方差对传播后的Sigma点集进行加权求和得到k时刻的先验状态估计x_{k|k-1}和先验估计协方差P_{k|k-1}。# 计算预测均值 x_pred np.zeros(n) for i in range(2*n1): x_pred W_m[i] * predicted_sigma_points[i] # 计算预测协方差 P_pred np.zeros((n, n)) for i in range(2*n1): y predicted_sigma_points[i] - x_pred P_pred W_c[i] * np.outer(y, y) # 外积 y * y^T P_pred Q # 加上过程噪声协方差这里加上Q是关键它代表了模型的不确定性和未建模动态使得先验协方差“膨胀”告诉滤波器要更相信即将到来的观测。3.3 更新步测量更新更新步的目标是利用当前时刻的实际观测值z_k来修正预测步得到的结果。步骤4根据预测状态再次计算Sigma点可选但常见有些实现会直接用预测的均值和协方差(x_pred, P_pred)生成一套新的Sigma点用于观测方程的传播。另一种做法是复用预测步中传播后的Sigma点X_{k|k-1}。前者更常见因为它严格遵循了UT的流程基于当前的最佳估计先验分布进行采样。# 基于先验估计 (x_pred, P_pred) 生成一套新的Sigma点 sigma_points_pred, W_m, W_c compute_sigma_points(x_pred, P_pred)步骤5Sigma点通过观测函数将Sigma点X_{k|k-1}^{(i)}通过非线性观测方程h进行传播得到预测的观测Sigma点Z_{k}^{(i)}。predicted_obs_sigma_points np.zeros((2*n1, m)) # m 是观测维度 for i in range(2*n1): predicted_obs_sigma_points[i] h(sigma_points_pred[i])步骤6计算预测观测的均值与协方差# 预测观测的均值 z_pred np.zeros(m) for i in range(2*n1): z_pred W_m[i] * predicted_obs_sigma_points[i] # 预测观测的协方差 (Innovation Covariance) P_zz np.zeros((m, m)) for i in range(2*n1): y predicted_obs_sigma_points[i] - z_pred P_zz W_c[i] * np.outer(y, y) P_zz R # 加上观测噪声协方差 # 状态与观测的互协方差 P_xz np.zeros((n, m)) for i in range(2*n1): x_diff sigma_points_pred[i] - x_pred z_diff predicted_obs_sigma_points[i] - z_pred P_xz W_c[i] * np.outer(x_diff, z_diff)步骤7计算卡尔曼增益更新状态与协方差这一步和标准卡尔曼滤波形式完全一致只是计算P_zz和P_xz的方法不同。# 卡尔曼增益 K np.dot(P_xz, np.linalg.inv(P_zz)) # 实际观测值 z_actual np.array([...]) # 状态更新 x_updated x_pred np.dot(K, (z_actual - z_pred)) # 协方差更新 (Joseph form 更稳定) I np.eye(n) P_updated np.dot((I - np.dot(K, H.T)), P_pred) # 简单形式可能不对称 # 更稳健的约瑟夫形式 # P_updated np.dot((I - np.dot(K, H.T)), P_pred).dot((I - np.dot(K, H.T)).T) np.dot(K, R).dot(K.T) # 对于UKF更常用的是 P_updated P_pred - np.dot(K, np.dot(P_zz, K.T))至此我们就完成了UKF的一次完整迭代。x_updated和P_updated就是k时刻的后验估计将作为下一轮预测步的输入。4. 代码实战手把手实现一个简易UKF滤波器理论讲得再多不如一行代码。下面我们用Python实现一个针对一维非线性系统的UKF来估计一个衰减振荡信号。这个例子非常经典能直观展示UKF处理非线性的能力。假设我们有一个简单的非线性系统状态方程x_k sin(x_{k-1}) q这是一个非线性状态转移。观测方程z_k x_k^2 r这是一个非线性观测。 过程噪声q ~ N(0, 0.1)观测噪声r ~ N(0, 1)。我们的目标是仅通过带噪声的观测z来估计真实的状态x。import numpy as np import matplotlib.pyplot as plt class SimpleUKF: def __init__(self, dim_x, dim_z, Q, R, alpha1e-3, beta2, kappa0): self.dim_x dim_x # 状态维度 self.dim_z dim_z # 观测维度 self.Q Q # 过程噪声协方差 self.R R # 观测噪声协方差 self.alpha alpha self.beta beta self.kappa kappa self.lambda_ alpha**2 * (dim_x kappa) - dim_x # 权重初始化 self.W_m, self.W_c self._compute_weights() # 状态初始化 (将在第一次更新时进行) self.x np.zeros(dim_x) self.P np.eye(dim_x) def _compute_weights(self): n self.dim_x lambda_ self.lambda_ W_m np.zeros(2*n1) W_c np.zeros(2*n1) W_m[0] lambda_ / (n lambda_) W_c[0] W_m[0] (1 - self.alpha**2 self.beta) for i in range(1, 2*n1): W_m[i] 1.0 / (2*(n lambda_)) W_c[i] W_m[i] return W_m, W_c def _compute_sigma_points(self, x, P): n self.dim_x lambda_ self.lambda_ # 计算矩阵平方根 # 使用乔列斯基分解并处理可能的非正定情况 try: U np.linalg.cholesky((n lambda_) * P) except np.linalg.LinAlgError: # 如果分解失败给P加上一个小的正则项 U np.linalg.cholesky((n lambda_) * P 1e-8 * np.eye(n)) sigma_points np.zeros((2*n1, n)) sigma_points[0] x for i in range(n): sigma_points[i1] x U[i] sigma_points[ni1] x - U[i] return sigma_points def predict(self, f, uNone): 预测步 Args: f: 非线性状态转移函数签名 f(x, u) u: 控制输入 (可选) # 1. 生成Sigma点 sigma_points self._compute_sigma_points(self.x, self.P) # 2. 通过状态转移函数传播Sigma点 n self.dim_x predicted_points np.zeros((2*n1, n)) for i in range(2*n1): predicted_points[i] f(sigma_points[i], u) # 3. 计算预测均值和协方差 self.x_pred np.zeros(n) for i in range(2*n1): self.x_pred self.W_m[i] * predicted_points[i] self.P_pred np.zeros((n, n)) for i in range(2*n1): diff predicted_points[i] - self.x_pred self.P_pred self.W_c[i] * np.outer(diff, diff) self.P_pred self.Q # 保存预测的Sigma点可供更新步使用可选 self.sigma_points_pred self._compute_sigma_points(self.x_pred, self.P_pred) return self.x_pred, self.P_pred def update(self, z, h): 更新步 Args: z: 观测值维度 (dim_z,) h: 非线性观测函数签名 h(x) if not hasattr(self, sigma_points_pred): # 如果预测步没有生成新的Sigma点则基于当前预测生成 self.sigma_points_pred self._compute_sigma_points(self.x_pred, self.P_pred) n self.dim_x m self.dim_z sigma_points self.sigma_points_pred # 1. 通过观测函数传播Sigma点 predicted_obs np.zeros((2*n1, m)) for i in range(2*n1): predicted_obs[i] h(sigma_points[i]) # 2. 计算预测观测的统计量 z_pred np.zeros(m) for i in range(2*n1): z_pred self.W_m[i] * predicted_obs[i] P_zz np.zeros((m, m)) P_xz np.zeros((n, m)) for i in range(2*n1): # 观测误差 z_diff predicted_obs[i] - z_pred P_zz self.W_c[i] * np.outer(z_diff, z_diff) # 状态与观测的互协方差 x_diff sigma_points[i] - self.x_pred P_xz self.W_c[i] * np.outer(x_diff, z_diff) P_zz self.R # 加入观测噪声 # 3. 卡尔曼增益 K np.dot(P_xz, np.linalg.inv(P_zz)) # 4. 更新状态和协方差 y z - z_pred # 新息 (Innovation) self.x self.x_pred np.dot(K, y) self.P self.P_pred - np.dot(K, np.dot(P_zz, K.T)) return self.x, self.P # 定义非线性函数 def f_state(x, uNone): 状态转移函数: x_k sin(x_{k-1}) return np.sin(x) def h_obs(x): 观测函数: z_k x_k^2 return x**2 # 生成仿真数据 np.random.seed(42) steps 200 true_state np.zeros(steps) obs np.zeros(steps) true_state[0] 0.5 # 初始真实状态 for t in range(1, steps): true_state[t] np.sin(true_state[t-1]) np.random.normal(0, np.sqrt(0.1)) # 加过程噪声 obs[t] true_state[t]**2 np.random.normal(0, 1) # 加观测噪声 # 初始化UKF ukf SimpleUKF(dim_x1, dim_z1, Qnp.array([[0.1]]), # 过程噪声方差 Rnp.array([[1.0]]), # 观测噪声方差 alpha1e-3, beta2, kappa0) ukf.x np.array([0.0]) # 初始状态估计 ukf.P np.array([[1.0]]) # 初始协方差 # 运行滤波 estimated_state np.zeros(steps) for t in range(steps): # 预测 ukf.predict(f_state) # 更新 estimated_state[t], _ ukf.update(np.array([obs[t]]), h_obs) # 绘图 plt.figure(figsize(12, 6)) plt.plot(true_state, g-, labelTrue State, linewidth2) plt.plot(obs, r., labelNoisy Observation, markersize4, alpha0.6) plt.plot(estimated_state, b-, labelUKF Estimate, linewidth1.5) plt.xlabel(Time Step) plt.ylabel(State Value) plt.title(UKF Estimation for Nonlinear System: x_k sin(x_{k-1}), z_k x_k^2) plt.legend() plt.grid(True) plt.show()运行这段代码你会看到UKF的估计结果蓝色线能够很好地跟踪真实状态绿色线尽管观测红点是非线性且噪声很大的。这个例子清晰地展示了UKF如何绕过雅可比矩阵直接处理非线性关系。5. UKF参数调优与工程实践中的坑理论完美代码也跑通了但把UKF应用到实际项目时你会发现它远非“即插即用”。参数调优和工程实现细节决定了滤波器的成败。下面分享几个我踩过的坑和总结的经验。5.1 关键参数α, β, κ 的选择这三个参数决定了Sigma点的散布和权重。α (Alpha): 控制Sigma点围绕均值的散布范围。通常设置为一个很小的正数如1e-3。值越小Sigma点越靠近均值对非线性函数的近似越“局部化”值越大最大为1则散布越广。对于高度非线性的系统可以适当调大α例如0.5到1让Sigma点能探索到更远的区域但要注意不能太大否则会引入不必要的采样误差。我的经验是从1e-3开始如果滤波器在状态突变时反应迟钝或发散可以尝试缓慢增大。β (Beta): 用于融合分布的高阶矩信息。对于高斯分布β2是最优的。对于其他分布可以调整。在大多数工程应用中我们默认状态和噪声服从高斯分布所以β2是一个安全且常用的选择。κ (Kappa): 次要缩放参数通常设为0或3-n其中n是状态维度。κ3-n的设定是为了让Sigma点的采样与分布的第四阶矩匹配。对于状态维度不高n3的系统这个设置比较合理。对于高维系统我通常直接设为0让α和β起主要调节作用。实操建议除非有特别理由否则使用默认值(α1e-3, β2, κ0)作为起点。将主要精力放在调Q和R上。5.2 过程噪声Q与观测噪声R的调参艺术Q和R是滤波器的“信任杠杆”是调参的核心。过程噪声协方差 Q它表示你对状态转移模型f(x)的信任程度。Q越大表示你认为模型不确定性越大滤波器会更倾向于相信观测值新息y的权重更大。如果你的模型非常精确Q应该设小如果模型粗糙或者系统有未建模的动态Q需要设大。观测噪声协方差 R它表示你对传感器观测z的信任程度。R越大表示你认为观测噪声大、不可靠滤波器会更倾向于相信自己的预测新息的权重更小。这需要根据传感器数据手册或实际测试来确定。如何调参理论值优先如果传感器厂商提供了噪声特性如标准差σ则R diag(σ²)。过程噪声Q可以根据系统物理特性或仿真来估计。新息序列检验这是最实用的方法。新息Innovationy z - z_pred在滤波器最优时应该是一个零均值的白噪声序列。你可以运行滤波器一段时间然后计算新息序列的均值应接近0和自相关应只在0滞后处有峰值。如果均值偏离0可能Q或R的比值不对如果自相关非白可能模型结构有问题或Q设小了。试错法这是一个经验过程。如果滤波器估计结果过于平滑跟不上真实状态的变化“滞后”说明滤波器太相信模型了可以尝试增大Q或减小R。如果估计结果抖动非常厉害对观测噪声过于敏感说明滤波器太相信观测了可以尝试减小Q或增大R。5.3 数值稳定性协方差矩阵的正定性保障这是UKF实现中最容易出问题的地方。在计算P_pred和P_updated时由于浮点数误差和权重可能为负当λ为负时协方差矩阵可能失去正定性导致下一次迭代的乔列斯基分解失败。防御性编程技巧使用约瑟夫形式更新协方差在更新步使用P (I-KH)P_pred(I-KH)^T KRK^T的形式虽然计算量稍大但能保证对称正定性。我在代码示例中使用了简化形式P P_pred - K P_zz K^T这在数值上可能不稳定。对协方差矩阵进行强制对称化每次更新后执行P (P P.T) / 2。在乔列斯基分解前添加正则项如果分解失败在矩阵P上加上一个很小的单位矩阵ϵI如1e-8如代码示例中的try-except块所示。使用平方根UKF (SR-UKF)这是更高级、更稳定的实现。它直接对协方差矩阵的平方根进行更新避免了协方差矩阵本身可能出现的非正定问题。工业级代码如机器人操作系统ROS中的robot_localization包通常会采用SR-UKF或其变种。5.4 UKF与EKF、粒子滤波的对比与选型最后我们聊聊什么时候该用UKF。vs. 扩展卡尔曼滤波 (EKF)优势UKF无需计算雅可比矩阵实现更简单尤其适用于导数难求或不可求的系统。对于中度非线性系统UKF的估计精度通常优于或等于EKF且稳定性更好。劣势计算量比EKF大。EKF需要计算O(n²)量级的雅可比矩阵并相乘而UKF需要2n1次函数评估。对于状态维度n很高的系统如大型SLAM问题UKF的计算成本可能成为瓶颈。不过对于n在10以下的大多数嵌入式系统如无人机、机器人2n1次评估的代价是可以接受的。vs. 粒子滤波 (PF)优势UKF是确定性采样只用2n1个点计算效率远高于需要数百上千个粒子的PF。UKF对于高斯噪声的假设下是最优或次优的。劣势UKF基于高斯假设。如果真实状态分布是高度非高斯、多模态的比如机器人 kidnapped problemUKF会失效因为它用一个高斯分布去近似会丢失多峰信息。而粒子滤波没有这个限制。选型指南如果你的系统非线性程度一般且状态噪声基本符合高斯分布EKF是首选因为它最成熟、计算量最小。如果你的系统非线性较强但维度不高n10且仍可假设为高斯分布UKF是更好的选择它能提供更稳定、更精确的估计且实现上避免了求导的麻烦。如果你的系统状态分布明显非高斯多峰、强偏态或者模型极度非线性那么应该考虑粒子滤波或其他非线性滤波方法。在我个人的工程经验里UKF在无人机姿态估计融合IMU和磁力计、车辆定位融合GPS、IMU、轮速计等场景中表现非常出色它很好地平衡了精度、稳定性和实现复杂度。当你被EKF的雅可比矩阵搞得焦头烂额时试试UKF或许会有惊喜。
)
