简介一份面向惯性导航、机器人与组合导航初学者的扩展卡尔曼滤波EKF与误差状态卡尔曼滤波ESKF实现资源内容来自论文《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU》可用于理解9轴IMU中加速度计、陀螺仪与磁力计的数据融合及姿态解算核心流程。压缩包约2.23MB代码结构按EKF-IMU和ESKF-IMU分别组织便于对照论文推导公式逐步验证状态预测、量测更新与误差反馈环节。已有441人学习资源既提供了可直接编译运行的算法实现也给出了面向论文思路的工程化拆解适合正在学习卡尔曼滤波理论、需要落地代码或调试姿态估计程序的读者参考。 做状态估计的这几年扩展卡尔曼滤波EKF是我用得最多的算法没有之一。它解决的是一个特别直白的场景你的系统是非线性的但你又想用卡尔曼滤波那套优雅的递推框架来估计系统状态。当你拿到一堆带噪声的传感器数据想知道系统内部真实状态是多少EKF通常是最先应该尝试的方案。它的应用面非常广——机器人定位、目标跟踪、组合导航、自动驾驶感知、无人机飞控、电池SOC估算凡是涉及非线性系统状态估计的地方基本都绕不开它。这篇文章适合所有写滤波算法的工程师和学生我会从原理推导讲到工程坑点最后给一个可以直接跑的雷达跟踪例程尽量让你看完就能上手。1. 为什么线性卡尔曼不够用非线性才是工程常态1.1 卡尔曼滤波的隐含前提很多人学卡尔曼滤波的时候教材上给的都是线性系统的标准形式状态转移是矩阵乘法观测也是矩阵乘法。但说实话我工作以后发现真正的工程系统里能严格写成线性矩阵形式的反而少见。卡尔曼滤波的推导建立在两个前提上系统是线性的噪声是高斯的。这两个前提凑在一起高斯分布经过线性变换之后还是高斯分布所以整个递推过程才完美闭环。问题在于现实中稍微复杂一点的系统状态转移或者观测方程里就会冒出非线性项。比如一个简单的二维目标跟踪目标状态是位置和速度你用雷达测它雷达直接输出的是距离和方位角。距离和方位角到笛卡尔坐标的换算里面有平方根、有反正切这不是线性变换。你要是硬套标准卡尔曼滤波要么把观测强行当线性处理要么只能在小角度近似下勉强用稍微动一动就崩。1.2 工程中的非线性从哪来我总结了一下工程里的非线性主要来自三个地方。运动模型的非线性最常见。比如机器人航迹推算dead reckoning状态里有航向角位置更新里必然是delta_x v * cos(theta) * dt、delta_y v * sin(theta) * dt这种带三角函数的项就是典型的非线性。还有带转弯率的运动模型转弯率本身会进到状态转移里模型天然就是非线性的。观测模型的非线性同样普遍。雷达、激光雷达输出极坐标量测相机输出像素坐标这些传感器拿到的是经过非线性几何变换后的数据。你要在滤波里把预测值和观测值做对比就必须处理这个非线性映射。第三种不太容易注意到但非常坑——坐标系变换。比如车辆定位里GPS给的是经纬度惯导给的是机体坐标系下的加速度毫米波雷达给的是以雷达为原点的极坐标量测这些数据要融合进同一个状态向量里中间全是非线性变换。1.3 EKF的基本思想在工作点附近做泰勒展开EKF的思路其实特别朴素既然系统是非线性的那我就在当前估计值附近做一阶泰勒展开把非线性函数局部线性化。打个比方地球表面是弯曲的但你站在地面上看局部就是平的。你只要不走太远用平面近似球面误差不大。EKF就是这个逻辑在每一个滤波时刻沿着当前估计状态这个“点”把非线性函数展开取一阶项丢掉高阶项得到局部的线性模型然后套标准卡尔曼滤波的公式。这个思路简单但非常有效。它不需要像全局线性化那样对整个状态空间做近似而是“边走边近似”状态估计到哪里就在哪里线性化。这就是EKF能用几十年还没被淘汰的根本原因。2. EKF的核心设计思路把非线性问题“局部线性化”2.1 预测与更新雅可比替换状态转移矩阵EKF的递推流程和标准卡尔曼滤波骨架是一样的依然是预测加更新两步。预测步里状态的一步预测直接走非线性函数x_pred f(x_est)。协方差预测则要靠雅可比矩阵F来代替标准卡尔曼里的状态转移矩阵P_pred F P_est F.T Q。这里的F是状态方程对状态向量求偏导得到的雅可比矩阵。更新步也类似。卡尔曼增益计算用的是观测方程的雅可比矩阵HK P_pred H.T (H P_pred H.T R)^-1。状态更新用的是真实观测值和预测观测值的差其中预测观测值是z_pred h(x_pred)也就是把预测状态通过非线性观测函数映射到量测空间再和传感器实测值做差。这一步是EKF的灵魂你比较的不是状态空间里的东西而是量测空间里的东西。预测状态映射到量测空间再和真实量测做差得到的就是“新息”innovation也就是卡尔曼滤波里用来修正预测的误差信号。2.2 一阶线性化为什么够用很多人第一次接触EKF会问只取一阶项精度够吗答案要看你的系统非线性强度。如果系统在估计点附近的一小段区域内非线性函数图像接近一条直线那一阶泰勒展开误差就很小EKF的表现会非常好。比如雷达目标跟踪目标距离比较远、角度变化不大时观测函数的局部线性性很好EKF能给出非常稳定的估计。如果系统非线性太强比如火箭姿态在大角度机动下的描述一阶展开丢掉的二阶项就不可忽视了。这时候EKF可能出现估计偏差甚至发散。但对大多数工程场景EKF仍然是性价比最高的选择。还有一个实际理由解析雅可比的计算虽然费点功夫但一旦推出来计算量就是纯矩阵运算非常适合嵌入式实时系统。我在MCU上跑过EKF状态维度8维的情况下单次滤波周期也就几十微秒级别这是加性无迹卡尔曼UKF和粒子滤波都做不到的实时性。2.3 什么时候该换UKF或粒子滤波EKF不是万能的我吃过几次亏之后总结出了几条判断标准。如果你的系统非线性特别强或者初始误差特别大一阶线性化的误差会直接让滤波器发散这时候可以考虑UKF无迹卡尔曼滤波。UKF通过Sigma点采样来近似概率分布不需要算雅可比对非线性系统的精度通常比EKF高一个量级而且实现一点也不复杂代码量甚至比EKF少因为不用推导数。如果系统是非高斯噪声或者状态分布呈多峰形态卡尔曼家族全都失效这时候只能上粒子滤波。粒子滤波用一堆随机样本近似任意分布理论上很强大但计算量巨大而且存在粒子退化问题工程上能用EKF解决的尽量不碰粒子滤波。我的个人经验是先在仿真环境里用同一个测试轨迹对比EKF和UKF的估计误差如果两者差距在可接受范围内就无脑选EKF毕竟它实时性最好、内存占用最小。真到了EKF误差大到不可接受的时候再升级到UKF也不迟。3. 雅可比矩阵与噪声矩阵EKF最容易翻车的细节3.1 雅可比矩阵的解析推导雅可比矩阵是EKF里最容易出错的地方而且错了还很难发现因为滤波可能只是在收敛速度或精度上变差不会直接报错。雅可比矩阵的本质是偏导数矩阵。状态方程的雅可比F是状态函数对状态向量每个分量的偏导观测方程的雅可比H是观测函数对状态向量每个分量的偏导。我以一个典型例子来说明。假设系统状态是笛卡尔坐标下的位置和速度x [px, py, vx, vy]观测是雷达测得的距离r和方位角theta。观测方程为r sqrt(px^2 py^2) theta atan2(py, px)观测方程对状态求偏导得到观测雅可比矩阵HH [ [px/r, py/r, 0, 0], [-py/(px^2py^2), px/(px^2py^2), 0, 0] ]第一行是距离对位置的偏导第二行是方位角对位置的偏导。注意atan2对整个二维平面都有定义不能简单用arctan(py/px)否则在px接近0的地方会出问题雅可比也会算错。做解析推导的时候我的习惯是把推导过程写在注释里比如在代码里注明“这是h对px求偏导的结果”方便后面排查。很多工程事故都源于某一行偏导数符号写错或者漏了某个链式法则项有注释会好查很多。3.2 数值雅可比的步长怎么选有时候状态方程或观测方程太复杂解析推导实在推不动可以用数值差分代替。但数值雅可比有讲究核心是差分步长的选取。步长太大差分近似误差大而且可能跨过非线性函数的弯曲区域步长太小会陷入浮点精度困境两个相近的数相减直接消掉了有效数字。我常用的步长是sqrt(eps) * max(1, abs(x_i))其中eps是机器精度双精度下约2.2e-16算下来大概是1e-8乘以状态分量的量级。这本质上是平衡截断误差和舍入误差很多人不知道这个公式随手取个1e-3滤波效果差得离谱还找不到原因。不过我还是建议尽量做解析推导。数值雅可比在每一拍都要做N次额外的状态函数计算计算量翻倍而且引入了额外的数值噪声。只有状态方程实在太复杂、解析推导容易出错的时候我才会用数值法做交叉验证同一组数据分别用解析和数值雅可比跑一遍如果结果差异很大那肯定是某一个雅可比算错了。3.3 Q矩阵和R矩阵的设置经验过程噪声协方差Q和量测噪声协方差R是EKF里另一对容易让人翻车的参数。这两个矩阵的物理含义是明确的Q描述的是你对运动模型的信任程度R描述的是你对传感器量测的信任程度。R矩阵相对好办一点。传感器噪声通常可以从手册查或者做静态实验测出来。比如一个雷达的距离噪声标准差是0.1米、角度噪声标准差是0.5度那R就是对角阵diag([0.1^2, (0.5*pi/180)^2])。注意一定要把角度换算成弧度这个单位错误我见过不止一次。Q矩阵相对麻烦。它是模型不确定度的表现包括你忽略的加速度项、模型简化带来的误差、甚至计算舍入。我常用的一个方法是用连续白噪声加速度模型推导离散Q矩阵。比如近匀速模型假设加速度噪声强度为q那么一步预测的Q矩阵是Q q * [ [dt^3/3, 0, dt^2/2, 0], [0, dt^3/3, 0, dt^2/2], [dt^2/2, 0, dt, 0], [0, dt^2/2, 0, dt] ]这个矩阵不是拍脑袋拍出来的而是把“加速度是方差为q的白噪声”这个假设积分得到的。很多教材直接给结果不告诉你怎么来的实际调参的时候你不知道改哪个数。理解来源之后调参就有方向了如果目标真实机动比较大就调大q如果目标运动很平稳q可以调很小。4. 一个可以直接跑的雷达跟踪例程从公式到代码4.1 场景与模型定义用前面讲的近匀速模型加雷达观测我写一个完整的跟踪例程。场景是这样的一个目标在二维平面上近似匀速直线运动雷达固定在原点每个周期测出目标的距离和方位角。我们的任务是实时估计目标在笛卡尔坐标系下的位置和速度。状态向量是四维[px, py, vx, vy]。运动模型用近匀速模型状态转移矩阵是线性矩阵因为匀速运动本身是线性的但观测模型是非线性的——极坐标测量就是前面推过雅可比的那个函数。这个场景虽然简单但把EKF最关键的非线性观测部分讲透了实际工程里雷达跟踪、声呐跟踪、激光雷达目标跟踪基本都是这个套路。仿真参数我设成采样周期dt0.1秒跑500步共50秒目标的真实初始位置在(1000, 1000)米速度为(10, -5)米/秒左右。量测噪声标准差距离0.5米角度0.5度。过程噪声强度q设0.1。需要说明的是我这里模拟的目标其实带一点小幅度的随机加速度用来模拟模型不完美的情况这样更接近真实也方便看EKF的修正能力。4.2 核心代码实现下面给出核心的EKF循环代码用numpy实现注释写得很详细可以直接拷到项目里改参数用import numpy as np dt 0.1 q 0.1 # 状态转移矩阵 F近匀速模型 F np.array([ [1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1] ]) # 过程噪声协方差 Q来自连续白噪声加速度模型 Q q * np.array([ [dt**3/3, 0, dt**2/2, 0], [0, dt**3/3, 0, dt**2/2], [dt**2/2, 0, dt, 0], [0, dt**2/2, 0, dt] ]) # 量测噪声协方差 R R np.diag([0.5**2, (0.5 * np.pi / 180)**2]) # 状态初值用第一帧量测初始化位置速度给0 x np.array([1000.0, 1000.0, 0.0, 0.0]) P np.diag([10.0, 10.0, 5.0, 5.0]) def h(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 H_jacobian(x): px, py x[0], x[1] r np.sqrt(px**2 py**2) r2 px**2 py**2 # 第一行距离对px、py的偏导 # 第二行方位角对px、py的偏导 return np.array([ [px / r, py / r, 0, 0], [-py / r2, px / r2, 0, 0] ]) def ekf_predict(x, P, F, Q): x_pred F x P_pred F P F.T Q return x_pred, P_pred def ekf_update(x_pred, P_pred, z, H, R): # 计算观测预测和雅可比矩阵 z_pred h(x_pred) Hk H_jacobian(x_pred) # 新息innovation # 把预测状态映射到量测空间再减去真实量测 # 注意角度残差需要归一到 [-pi, pi] y z - z_pred y[1] np.arctan2(np.sin(y[1]), np.cos(y[1])) S Hk P_pred Hk.T R K P_pred Hk.T np.linalg.inv(S) x_new x_pred K y # Joseph 形式的协方差更新数值上更稳定 I np.eye(len(x)) P_new (I - K Hk) P_pred (I - K Hk).T K R K.T return x_new, P_new # 主循环里就是 predict 再 update # for each measurement z: # x_pred, P_pred ekf_predict(x, P, F, Q) # x, P ekf_update(x_pred, P_pred, z, H, R)这段代码看起来不长但完整包含了EKF的所有关键步骤。注意协方差更新我特意用了Joseph形式而不是常见的P (I - K H) P_pred这个细节在4.3节和5.3节会详细说。4.3 运行效果与参数调试跑完仿真把估计轨迹和真实轨迹画出来看你会发现最初几步位置估计误差快速收敛到几米以内速度估计也慢慢逼近真实值。过程中如果目标偶尔来一个机动EKF会先跟上一点但输出轨迹会出现一个小“凹陷”然后迅速修正回来。调参方面我第一次跑这个例程的时候故意只调q不调R感受非常直观q调小到0.001滤波结果平滑得像丝一样但一旦目标转弯或者有加速度误差立刻被拉大且恢复很慢这是模型过度自信的表现。q调大到10滤波结果几乎不再平滑每条量测噪声都直接进入了估计轨迹失去了滤波的意义。所以调参口诀我这几年总结下来就一句话先固定R从真实传感器噪声出发再调q让滤波结果在平滑性和响应速度之间取一个平衡点。最好跑一段有代表性的数据不断对比估计值和真值之间的残差。记录每一次参数变了什么、结果曲线怎么变调参就会越来越快。5. 常见问题与排查技巧滤波发散、角度环绕和数值稳定性5.1 滤波器发散怎么查滤波器发散是最让人头皮发麻的问题运行着运行着估计值突然飞了或者协方差矩阵变得不正常。我总结了一套排查路径按顺序走大部分问题都能定位。第一步看数据源。量测有没有异常跳变时间戳是否连续很多发散其实是传感器本身丢包或者给了异常大值和滤波算法一点关系都没有。第二步打印每一拍的新息序列如果新息均值明显不为零、或者标准差远超sqrt(S)的理论值说明滤波器的预测或者量测模型出了问题。第三步检查雅可比矩阵拿数值差分和解析结果对比一下很可能就是某个偏导数写错了。还有一个隐蔽问题Q设置得太小。模型不确定度被严重低估导致协方差P越收越小滤波器变得越来越“自信”新息稍大一点就会做出剧烈修正最后振荡发散。这是新手最容易踩的坑遇到发散先别怀疑公式先把q调大一个数量级试跑一次。5.2 角度环绕问题角度环绕是我觉得EKF里最经典、最隐蔽的一个坑值得单独拿出来说。在雷达跟踪例程中方位角的范围是[-pi, pi]。假设真实方位角是179度预测值是-179度差值是358度。如果不做处理滤波器会认为误差极大猛地修正一下结果直接偏掉。但实际上真实误差只有2度。解决办法就是角度归一化计算残差后把残差通过atan2(sin(残差), cos(残差))映射到[-pi, pi]。我在前面的代码里已经写了这一行但很多人第一次写的时候都会忽略它。只要角度参与滤波不管是EKF、UKF还是粒子滤波都必须做这个处理。这个问题在INS/组合导航里更严重因为航向角、姿态角全是周期量一个没注意滤波器直接原地爆炸。5.3 EKF的数值稳定性技巧EKF的协方差更新在理论上是保持对称正定性的但实际浮点计算中P (I - K H) P_pred这种形式容易让P矩阵慢慢变得不对称甚至出现负的特征值最终发散。解决办法有两个。一是在每次更新后用P (P P.T) / 2强制对称化计算量忽略不计但能有效防止数值恶化。二是我在上面代码里用的方法——Joseph形式更新。它利用了公式展开的对称性能更好地保持协方差的非负定性。代价是多两次矩阵乘法和一次矩阵加法计算量增加不多但在嵌入式平台上如果时间预算吃紧可以先做对称化处理不用Joseph形式。还有一个技巧即使协方差矩阵在数学上应该正定数值上依然可能出现奇异。可以为P的对角元设置一个很小的下界比如1e-12防止某些状态分量完全失去不确定性导致卡尔曼增益数值爆炸。这是一种“工程保险”虽然不优雅但在实际系统中非常管用。5.4 一个判断EKF是否够用的土办法最后分享一个我一直在用的判断方法。EKF的线性化误差最终都会体现在新息序列上。所以我可以做个“新息白噪声检验”如果EKF工作正常新息序列应当近似零均值、无相关性、方差符合S H P_pred H.T R的理论值。实操上很粗暴跑一段仿真数据把每一拍的新息存下来画自相关图。如果自相关在滞后1拍以后基本为零说明模型和滤波器匹配良好EKF够用。如果自相关显著不为零说明新息里有能用上一拍信息预测出来的成分这意味着系统有未建模的动态或者线性化误差过大这时候就要考虑增强模型、改用UKF或者上迭代EKF。这个方法不花什么成本但能帮你把“感觉滤波效果不太好”变成一个可以量化的判断。我在项目里把它当成EKF上线前的体检项目每次都能提前发现几个潜在的模型问题。最后再说一个个人习惯每次搭建EKF这种状态估计器我都会用纯仿真数据先验证滤波逻辑再掺入一定程度的噪声最后才接真实传感器数据。因为真实数据问题太多——时间戳抖动、坐标转换误差、传感器自身故障如果滤波逻辑本身还没调干净就上真机出了问题你根本不知道是算法问题还是数据问题。老老实实把仿真环境里的定位误差调到理论下界附近再碰真数据会省下很多排查时间。本文还有配套的精品资源点击获取 扩展卡尔曼滤波（EKF）核心原理、工程细节与雷达目标跟踪实战
简介一份面向惯性导航、机器人与组合导航初学者的扩展卡尔曼滤波EKF与误差状态卡尔曼滤波ESKF实现资源内容来自论文《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU》可用于理解9轴IMU中加速度计、陀螺仪与磁力计的数据融合及姿态解算核心流程。压缩包约2.23MB代码结构按EKF-IMU和ESKF-IMU分别组织便于对照论文推导公式逐步验证状态预测、量测更新与误差反馈环节。已有441人学习资源既提供了可直接编译运行的算法实现也给出了面向论文思路的工程化拆解适合正在学习卡尔曼滤波理论、需要落地代码或调试姿态估计程序的读者参考。 做状态估计的这几年扩展卡尔曼滤波EKF是我用得最多的算法没有之一。它解决的是一个特别直白的场景你的系统是非线性的但你又想用卡尔曼滤波那套优雅的递推框架来估计系统状态。当你拿到一堆带噪声的传感器数据想知道系统内部真实状态是多少EKF通常是最先应该尝试的方案。它的应用面非常广——机器人定位、目标跟踪、组合导航、自动驾驶感知、无人机飞控、电池SOC估算凡是涉及非线性系统状态估计的地方基本都绕不开它。这篇文章适合所有写滤波算法的工程师和学生我会从原理推导讲到工程坑点最后给一个可以直接跑的雷达跟踪例程尽量让你看完就能上手。1. 为什么线性卡尔曼不够用非线性才是工程常态1.1 卡尔曼滤波的隐含前提很多人学卡尔曼滤波的时候教材上给的都是线性系统的标准形式状态转移是矩阵乘法观测也是矩阵乘法。但说实话我工作以后发现真正的工程系统里能严格写成线性矩阵形式的反而少见。卡尔曼滤波的推导建立在两个前提上系统是线性的噪声是高斯的。这两个前提凑在一起高斯分布经过线性变换之后还是高斯分布所以整个递推过程才完美闭环。问题在于现实中稍微复杂一点的系统状态转移或者观测方程里就会冒出非线性项。比如一个简单的二维目标跟踪目标状态是位置和速度你用雷达测它雷达直接输出的是距离和方位角。距离和方位角到笛卡尔坐标的换算里面有平方根、有反正切这不是线性变换。你要是硬套标准卡尔曼滤波要么把观测强行当线性处理要么只能在小角度近似下勉强用稍微动一动就崩。1.2 工程中的非线性从哪来我总结了一下工程里的非线性主要来自三个地方。运动模型的非线性最常见。比如机器人航迹推算dead reckoning状态里有航向角位置更新里必然是delta_x v * cos(theta) * dt、delta_y v * sin(theta) * dt这种带三角函数的项就是典型的非线性。还有带转弯率的运动模型转弯率本身会进到状态转移里模型天然就是非线性的。观测模型的非线性同样普遍。雷达、激光雷达输出极坐标量测相机输出像素坐标这些传感器拿到的是经过非线性几何变换后的数据。你要在滤波里把预测值和观测值做对比就必须处理这个非线性映射。第三种不太容易注意到但非常坑——坐标系变换。比如车辆定位里GPS给的是经纬度惯导给的是机体坐标系下的加速度毫米波雷达给的是以雷达为原点的极坐标量测这些数据要融合进同一个状态向量里中间全是非线性变换。1.3 EKF的基本思想在工作点附近做泰勒展开EKF的思路其实特别朴素既然系统是非线性的那我就在当前估计值附近做一阶泰勒展开把非线性函数局部线性化。打个比方地球表面是弯曲的但你站在地面上看局部就是平的。你只要不走太远用平面近似球面误差不大。EKF就是这个逻辑在每一个滤波时刻沿着当前估计状态这个“点”把非线性函数展开取一阶项丢掉高阶项得到局部的线性模型然后套标准卡尔曼滤波的公式。这个思路简单但非常有效。它不需要像全局线性化那样对整个状态空间做近似而是“边走边近似”状态估计到哪里就在哪里线性化。这就是EKF能用几十年还没被淘汰的根本原因。2. EKF的核心设计思路把非线性问题“局部线性化”2.1 预测与更新雅可比替换状态转移矩阵EKF的递推流程和标准卡尔曼滤波骨架是一样的依然是预测加更新两步。预测步里状态的一步预测直接走非线性函数x_pred f(x_est)。协方差预测则要靠雅可比矩阵F来代替标准卡尔曼里的状态转移矩阵P_pred F P_est F.T Q。这里的F是状态方程对状态向量求偏导得到的雅可比矩阵。更新步也类似。卡尔曼增益计算用的是观测方程的雅可比矩阵HK P_pred H.T (H P_pred H.T R)^-1。状态更新用的是真实观测值和预测观测值的差其中预测观测值是z_pred h(x_pred)也就是把预测状态通过非线性观测函数映射到量测空间再和传感器实测值做差。这一步是EKF的灵魂你比较的不是状态空间里的东西而是量测空间里的东西。预测状态映射到量测空间再和真实量测做差得到的就是“新息”innovation也就是卡尔曼滤波里用来修正预测的误差信号。2.2 一阶线性化为什么够用很多人第一次接触EKF会问只取一阶项精度够吗答案要看你的系统非线性强度。如果系统在估计点附近的一小段区域内非线性函数图像接近一条直线那一阶泰勒展开误差就很小EKF的表现会非常好。比如雷达目标跟踪目标距离比较远、角度变化不大时观测函数的局部线性性很好EKF能给出非常稳定的估计。如果系统非线性太强比如火箭姿态在大角度机动下的描述一阶展开丢掉的二阶项就不可忽视了。这时候EKF可能出现估计偏差甚至发散。但对大多数工程场景EKF仍然是性价比最高的选择。还有一个实际理由解析雅可比的计算虽然费点功夫但一旦推出来计算量就是纯矩阵运算非常适合嵌入式实时系统。我在MCU上跑过EKF状态维度8维的情况下单次滤波周期也就几十微秒级别这是加性无迹卡尔曼UKF和粒子滤波都做不到的实时性。2.3 什么时候该换UKF或粒子滤波EKF不是万能的我吃过几次亏之后总结出了几条判断标准。如果你的系统非线性特别强或者初始误差特别大一阶线性化的误差会直接让滤波器发散这时候可以考虑UKF无迹卡尔曼滤波。UKF通过Sigma点采样来近似概率分布不需要算雅可比对非线性系统的精度通常比EKF高一个量级而且实现一点也不复杂代码量甚至比EKF少因为不用推导数。如果系统是非高斯噪声或者状态分布呈多峰形态卡尔曼家族全都失效这时候只能上粒子滤波。粒子滤波用一堆随机样本近似任意分布理论上很强大但计算量巨大而且存在粒子退化问题工程上能用EKF解决的尽量不碰粒子滤波。我的个人经验是先在仿真环境里用同一个测试轨迹对比EKF和UKF的估计误差如果两者差距在可接受范围内就无脑选EKF毕竟它实时性最好、内存占用最小。真到了EKF误差大到不可接受的时候再升级到UKF也不迟。3. 雅可比矩阵与噪声矩阵EKF最容易翻车的细节3.1 雅可比矩阵的解析推导雅可比矩阵是EKF里最容易出错的地方而且错了还很难发现因为滤波可能只是在收敛速度或精度上变差不会直接报错。雅可比矩阵的本质是偏导数矩阵。状态方程的雅可比F是状态函数对状态向量每个分量的偏导观测方程的雅可比H是观测函数对状态向量每个分量的偏导。我以一个典型例子来说明。假设系统状态是笛卡尔坐标下的位置和速度x [px, py, vx, vy]观测是雷达测得的距离r和方位角theta。观测方程为r sqrt(px^2 py^2) theta atan2(py, px)观测方程对状态求偏导得到观测雅可比矩阵HH [ [px/r, py/r, 0, 0], [-py/(px^2py^2), px/(px^2py^2), 0, 0] ]第一行是距离对位置的偏导第二行是方位角对位置的偏导。注意atan2对整个二维平面都有定义不能简单用arctan(py/px)否则在px接近0的地方会出问题雅可比也会算错。做解析推导的时候我的习惯是把推导过程写在注释里比如在代码里注明“这是h对px求偏导的结果”方便后面排查。很多工程事故都源于某一行偏导数符号写错或者漏了某个链式法则项有注释会好查很多。3.2 数值雅可比的步长怎么选有时候状态方程或观测方程太复杂解析推导实在推不动可以用数值差分代替。但数值雅可比有讲究核心是差分步长的选取。步长太大差分近似误差大而且可能跨过非线性函数的弯曲区域步长太小会陷入浮点精度困境两个相近的数相减直接消掉了有效数字。我常用的步长是sqrt(eps) * max(1, abs(x_i))其中eps是机器精度双精度下约2.2e-16算下来大概是1e-8乘以状态分量的量级。这本质上是平衡截断误差和舍入误差很多人不知道这个公式随手取个1e-3滤波效果差得离谱还找不到原因。不过我还是建议尽量做解析推导。数值雅可比在每一拍都要做N次额外的状态函数计算计算量翻倍而且引入了额外的数值噪声。只有状态方程实在太复杂、解析推导容易出错的时候我才会用数值法做交叉验证同一组数据分别用解析和数值雅可比跑一遍如果结果差异很大那肯定是某一个雅可比算错了。3.3 Q矩阵和R矩阵的设置经验过程噪声协方差Q和量测噪声协方差R是EKF里另一对容易让人翻车的参数。这两个矩阵的物理含义是明确的Q描述的是你对运动模型的信任程度R描述的是你对传感器量测的信任程度。R矩阵相对好办一点。传感器噪声通常可以从手册查或者做静态实验测出来。比如一个雷达的距离噪声标准差是0.1米、角度噪声标准差是0.5度那R就是对角阵diag([0.1^2, (0.5*pi/180)^2])。注意一定要把角度换算成弧度这个单位错误我见过不止一次。Q矩阵相对麻烦。它是模型不确定度的表现包括你忽略的加速度项、模型简化带来的误差、甚至计算舍入。我常用的一个方法是用连续白噪声加速度模型推导离散Q矩阵。比如近匀速模型假设加速度噪声强度为q那么一步预测的Q矩阵是Q q * [ [dt^3/3, 0, dt^2/2, 0], [0, dt^3/3, 0, dt^2/2], [dt^2/2, 0, dt, 0], [0, dt^2/2, 0, dt] ]这个矩阵不是拍脑袋拍出来的而是把“加速度是方差为q的白噪声”这个假设积分得到的。很多教材直接给结果不告诉你怎么来的实际调参的时候你不知道改哪个数。理解来源之后调参就有方向了如果目标真实机动比较大就调大q如果目标运动很平稳q可以调很小。4. 一个可以直接跑的雷达跟踪例程从公式到代码4.1 场景与模型定义用前面讲的近匀速模型加雷达观测我写一个完整的跟踪例程。场景是这样的一个目标在二维平面上近似匀速直线运动雷达固定在原点每个周期测出目标的距离和方位角。我们的任务是实时估计目标在笛卡尔坐标系下的位置和速度。状态向量是四维[px, py, vx, vy]。运动模型用近匀速模型状态转移矩阵是线性矩阵因为匀速运动本身是线性的但观测模型是非线性的——极坐标测量就是前面推过雅可比的那个函数。这个场景虽然简单但把EKF最关键的非线性观测部分讲透了实际工程里雷达跟踪、声呐跟踪、激光雷达目标跟踪基本都是这个套路。仿真参数我设成采样周期dt0.1秒跑500步共50秒目标的真实初始位置在(1000, 1000)米速度为(10, -5)米/秒左右。量测噪声标准差距离0.5米角度0.5度。过程噪声强度q设0.1。需要说明的是我这里模拟的目标其实带一点小幅度的随机加速度用来模拟模型不完美的情况这样更接近真实也方便看EKF的修正能力。4.2 核心代码实现下面给出核心的EKF循环代码用numpy实现注释写得很详细可以直接拷到项目里改参数用import numpy as np dt 0.1 q 0.1 # 状态转移矩阵 F近匀速模型 F np.array([ [1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1] ]) # 过程噪声协方差 Q来自连续白噪声加速度模型 Q q * np.array([ [dt**3/3, 0, dt**2/2, 0], [0, dt**3/3, 0, dt**2/2], [dt**2/2, 0, dt, 0], [0, dt**2/2, 0, dt] ]) # 量测噪声协方差 R R np.diag([0.5**2, (0.5 * np.pi / 180)**2]) # 状态初值用第一帧量测初始化位置速度给0 x np.array([1000.0, 1000.0, 0.0, 0.0]) P np.diag([10.0, 10.0, 5.0, 5.0]) def h(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 H_jacobian(x): px, py x[0], x[1] r np.sqrt(px**2 py**2) r2 px**2 py**2 # 第一行距离对px、py的偏导 # 第二行方位角对px、py的偏导 return np.array([ [px / r, py / r, 0, 0], [-py / r2, px / r2, 0, 0] ]) def ekf_predict(x, P, F, Q): x_pred F x P_pred F P F.T Q return x_pred, P_pred def ekf_update(x_pred, P_pred, z, H, R): # 计算观测预测和雅可比矩阵 z_pred h(x_pred) Hk H_jacobian(x_pred) # 新息innovation # 把预测状态映射到量测空间再减去真实量测 # 注意角度残差需要归一到 [-pi, pi] y z - z_pred y[1] np.arctan2(np.sin(y[1]), np.cos(y[1])) S Hk P_pred Hk.T R K P_pred Hk.T np.linalg.inv(S) x_new x_pred K y # Joseph 形式的协方差更新数值上更稳定 I np.eye(len(x)) P_new (I - K Hk) P_pred (I - K Hk).T K R K.T return x_new, P_new # 主循环里就是 predict 再 update # for each measurement z: # x_pred, P_pred ekf_predict(x, P, F, Q) # x, P ekf_update(x_pred, P_pred, z, H, R)这段代码看起来不长但完整包含了EKF的所有关键步骤。注意协方差更新我特意用了Joseph形式而不是常见的P (I - K H) P_pred这个细节在4.3节和5.3节会详细说。4.3 运行效果与参数调试跑完仿真把估计轨迹和真实轨迹画出来看你会发现最初几步位置估计误差快速收敛到几米以内速度估计也慢慢逼近真实值。过程中如果目标偶尔来一个机动EKF会先跟上一点但输出轨迹会出现一个小“凹陷”然后迅速修正回来。调参方面我第一次跑这个例程的时候故意只调q不调R感受非常直观q调小到0.001滤波结果平滑得像丝一样但一旦目标转弯或者有加速度误差立刻被拉大且恢复很慢这是模型过度自信的表现。q调大到10滤波结果几乎不再平滑每条量测噪声都直接进入了估计轨迹失去了滤波的意义。所以调参口诀我这几年总结下来就一句话先固定R从真实传感器噪声出发再调q让滤波结果在平滑性和响应速度之间取一个平衡点。最好跑一段有代表性的数据不断对比估计值和真值之间的残差。记录每一次参数变了什么、结果曲线怎么变调参就会越来越快。5. 常见问题与排查技巧滤波发散、角度环绕和数值稳定性5.1 滤波器发散怎么查滤波器发散是最让人头皮发麻的问题运行着运行着估计值突然飞了或者协方差矩阵变得不正常。我总结了一套排查路径按顺序走大部分问题都能定位。第一步看数据源。量测有没有异常跳变时间戳是否连续很多发散其实是传感器本身丢包或者给了异常大值和滤波算法一点关系都没有。第二步打印每一拍的新息序列如果新息均值明显不为零、或者标准差远超sqrt(S)的理论值说明滤波器的预测或者量测模型出了问题。第三步检查雅可比矩阵拿数值差分和解析结果对比一下很可能就是某个偏导数写错了。还有一个隐蔽问题Q设置得太小。模型不确定度被严重低估导致协方差P越收越小滤波器变得越来越“自信”新息稍大一点就会做出剧烈修正最后振荡发散。这是新手最容易踩的坑遇到发散先别怀疑公式先把q调大一个数量级试跑一次。5.2 角度环绕问题角度环绕是我觉得EKF里最经典、最隐蔽的一个坑值得单独拿出来说。在雷达跟踪例程中方位角的范围是[-pi, pi]。假设真实方位角是179度预测值是-179度差值是358度。如果不做处理滤波器会认为误差极大猛地修正一下结果直接偏掉。但实际上真实误差只有2度。解决办法就是角度归一化计算残差后把残差通过atan2(sin(残差), cos(残差))映射到[-pi, pi]。我在前面的代码里已经写了这一行但很多人第一次写的时候都会忽略它。只要角度参与滤波不管是EKF、UKF还是粒子滤波都必须做这个处理。这个问题在INS/组合导航里更严重因为航向角、姿态角全是周期量一个没注意滤波器直接原地爆炸。5.3 EKF的数值稳定性技巧EKF的协方差更新在理论上是保持对称正定性的但实际浮点计算中P (I - K H) P_pred这种形式容易让P矩阵慢慢变得不对称甚至出现负的特征值最终发散。解决办法有两个。一是在每次更新后用P (P P.T) / 2强制对称化计算量忽略不计但能有效防止数值恶化。二是我在上面代码里用的方法——Joseph形式更新。它利用了公式展开的对称性能更好地保持协方差的非负定性。代价是多两次矩阵乘法和一次矩阵加法计算量增加不多但在嵌入式平台上如果时间预算吃紧可以先做对称化处理不用Joseph形式。还有一个技巧即使协方差矩阵在数学上应该正定数值上依然可能出现奇异。可以为P的对角元设置一个很小的下界比如1e-12防止某些状态分量完全失去不确定性导致卡尔曼增益数值爆炸。这是一种“工程保险”虽然不优雅但在实际系统中非常管用。5.4 一个判断EKF是否够用的土办法最后分享一个我一直在用的判断方法。EKF的线性化误差最终都会体现在新息序列上。所以我可以做个“新息白噪声检验”如果EKF工作正常新息序列应当近似零均值、无相关性、方差符合S H P_pred H.T R的理论值。实操上很粗暴跑一段仿真数据把每一拍的新息存下来画自相关图。如果自相关在滞后1拍以后基本为零说明模型和滤波器匹配良好EKF够用。如果自相关显著不为零说明新息里有能用上一拍信息预测出来的成分这意味着系统有未建模的动态或者线性化误差过大这时候就要考虑增强模型、改用UKF或者上迭代EKF。这个方法不花什么成本但能帮你把“感觉滤波效果不太好”变成一个可以量化的判断。我在项目里把它当成EKF上线前的体检项目每次都能提前发现几个潜在的模型问题。最后再说一个个人习惯每次搭建EKF这种状态估计器我都会用纯仿真数据先验证滤波逻辑再掺入一定程度的噪声最后才接真实传感器数据。因为真实数据问题太多——时间戳抖动、坐标转换误差、传感器自身故障如果滤波逻辑本身还没调干净就上真机出了问题你根本不知道是算法问题还是数据问题。老老实实把仿真环境里的定位误差调到理论下界附近再碰真数据会省下很多排查时间。本文还有配套的精品资源点击获取