第168篇 无迹卡尔曼滤波UKF——不用算雅可比的替代方案

上篇讲了EKF——用泰勒展开把非线性函数线性化核心工具是雅可比矩阵。但EKF有个绕不开的问题雅可比矩阵难算、容易算错而且在强非线性系统中一阶近似的精度不够。有没有一种方法既不用算雅可比矩阵又能处理非线性系统有就是无迹卡尔曼滤波UKF。UKF的思路特别巧妙——与其线性化函数不如采样几个代表性的点把这些点直接扔进非线性函数然后看输出是什么。说白了就是不分析函数长什么样直接试探几个点来感受函数的行为。UT变换UKF的核心思想UKF的核心叫UT变换Unscented Transform。名字听着唬人其实原理很直觉。假设你有一个高斯分布的状态估计均值x和协方差P你想把这个状态通过一个非线性函数f变换之后得到新的均值和协方差。EKF的做法是对f做一阶泰勒展开来近似。UKF的做法完全不同从当前高斯分布中挑出2n1个特殊点叫sigma点n是状态维度把这2n1个点分别代入非线性函数f用变换后的2n1个点加权平均算出新的均值和协方差这就好比你想了解一个非线性函数在某个区域的行为与其用切线去近似不如直接在这个区域里撒几个点看看函数在这些点上的输出。sigma点的选取有讲究。它们不是随机选的而是根据均值和协方差精心计算的# sigma点生成n维状态2n1个点 chi_0 x_mean # 第0个点就是均值 chi_i x_mean sqrt((nkappa)*P) 的第i列 # i1..n chi_{ni} x_mean - sqrt((nkappa)*P)的第i列 # i1..n这里kappa是一个调节参数通常取0或者3-n。sqrt((nkappa)*P)是矩阵平方根通常用Cholesky分解来计算。UKF的完整流程UKF的流程和EKF类似也是预测-更新两步但内部操作不同。预测步中先对状态生成sigma点把每个sigma点代入非线性运动模型然后用加权平均得到预测均值和协方差# UKF预测步 sigma_points generate_sigma_points(x, P, n) for i in range(2*n1): sigma_pred[i] f(sigma_points[i], u) # 直接代入非线性函数 x_pred sum(W_m[i] * sigma_pred[i]) # 加权均值 P_pred sum(W_c[i] * outer(sigma_pred[i]-x_pred)) Q更新步同理对预测状态重新生成sigma点代入非线性观测函数计算预测测量值和协方差# UKF更新步 sigma_points generate_sigma_points(x_pred, P_pred, n) for i in range(2*n1): z_sigma[i] h(sigma_points[i]) # 代入观测函数 z_pred sum(W_m[i] * z_sigma[i]) P_zz sum(W_c[i] * outer(z_sigma[i]-z_pred)) R P_xz sum(W_c[i] * outer(sigma_points[i]-x_pred, z_sigma[i]-z_pred)) K P_xz inv(P_zz) # 卡尔曼增益 x_est x_pred K (z - z_pred) P_est P_pred - K P_zz K.T注意这里完全没有雅可比矩阵的身影——所有非线性函数都是直接代入计算的。UKF vs EKF到底选哪个面试中这个问题出现频率极高。两者的对比精度UKF通常比EKF精度更高。EKF是一阶近似UKF能捕捉到二阶效应因为sigma点在均值两侧对称分布能反映函数的曲率。在强非线性系统中UKF的优势更明显。计算量EKF需要计算雅可比矩阵解析推导或数值计算计算量取决于雅可比矩阵的复杂度。UKF需要生成和传播2n1个sigma点每个sigma点都要代入一次非线性函数。当状态维度n较小时比如n10UKF的计算量和EKF差不多。当n很大时UKF的计算量会明显大于EKF。实现难度UKF实现更简单——你只需要非线性函数本身不需要推导雅可比矩阵。EKF需要手推雅可比矩阵复杂系统中容易出错。稳定性EKF在强非线性系统中可能因为线性化误差太大而不稳定。UKF不存在线性化步骤通常更稳定。讲真在机器人领域状态维度一般不大6到20维UKF的计算量完全可接受。所以如果你不确定用EKF还是UKF选UKF通常不会错。工程中的调参经验UKF有几个关键参数需要调这些在教科书里基本找不到。kappasigma点的分布参数。Van der Merwe的经典论文推荐kappa 3 - nn是状态维度这个取值能保证sigma点的高阶矩匹配到四阶。实际中kappa0也常用效果差别不大。但如果你的非线性特别强可以试试把kappa调大一点让sigma点分布得更开一些这样能更好地探索非线性函数的曲率。alpha和beta如果用缩放参数形式alpha控制sigma点的分布范围通常取0.001到1之间。alpha越小sigma点越集中在均值附近线性化近似的味道就越重。beta和高斯分布的先验知识有关对于高斯分布beta2是最优的。数值稳定性这是UKF工程实现中最容易踩的坑。UKF中需要做Cholesky分解来求矩阵平方根。如果协方差矩阵P因为数值误差变得不正定Cholesky分解会直接报错。解决办法是在P上加一个小的正数比如1e-6 * I保证P的正定性。之前做项目的时候我们的UKF在跑了几千步之后突然崩溃报错说Cholesky分解失败。查了半天发现是协方差矩阵某个非对角线元素因为浮点累积误差变得不对称了。最后每步都强制做一次P (P P.T) / 2才解决。这种数值问题在仿真中可能很久才出现上了真车跑几个小时就炸特别阴险。面试中怎么聊面试官问UKF先说EKF的痛点雅可比难算、线性化误差再说UKF的核心思路sigma点采样代替线性化然后说UT变换的过程生成sigma点→代入非线性函数→加权统计最后对比UKF和EKF的优劣。如果面试官追问UKF为什么比EKF精度高你可以说EKF只用到了一阶泰勒展开忽略了二阶及以上的高阶项。UKF的sigma点对称分布在均值两侧能够捕捉到非线性函数的二阶效应。从数学上可以证明对于多项式函数UKF至少能精确到二阶具体精度取决于kappa的取值。如果面试官追问UKF的局限性你可以说UKF假设状态分布是高斯的和EKF一样在多模态分布场景下不适用。另外当状态维度很高时比如超过50维sigma点数量2n1会很大计算量会成为瓶颈。还有一种情况——如果非线性函数特别不规则比如有尖锐的间断点sigma点采样也可能不够准确。补充一个UKF工程实现的实用建议在调试UKF时建议先跑一个简单的一维或二维的仿真案例验证你的UT变换实现是否正确。具体方法是用一个已知的非线性函数比如yx²输入一个高斯分布的sigma点看输出的均值和方差是否和理论值吻合。如果一维案例通过了再逐步增加维度。这种从简单到复杂的调试策略能帮你快速定位问题是在UT变换本身还是在高维矩阵运算中。另外UKF跑完后一定要检查协方差矩阵P的对角线元素——如果某个状态分量的方差变成负数了说明数值稳定性出了问题需要加正则化项。下一篇讲粒子滤波——一种完全不同的思路不用高斯假设能处理多模态分布的非线性非高斯问题。如果这篇文章对你有帮助欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。「机器人软件开发面试·从入门到精通」连载系列上一篇第167篇 扩展卡尔曼滤波EKF——非线性系统的状态估计下一篇预告第169篇 粒子滤波——用撒豆子解决非线性非高斯定位有任何问题欢迎评论区留言我会尽量回复。