简介这份压缩包聚焦9维IMU姿态跟踪与多传感器融合包含扩展卡尔曼滤波EKF以及EKF-IMU、ESKF-IMU两类算法的实现并对应9维IMU集成处理器姿态跟踪中的双阶段卡尔曼滤波论文思路。内容面向机器人、导航定位、可穿戴设备与惯性导航方向的学习者可用于对照论文理解状态建模、观测更新、姿态解算与误差状态滤波流程并梳理滤波初始化、预测校正、协方差更新和姿态输出等关键环节。压缩包约2.23MB文件总数与主要文件类型明细暂无数据下载后可按目录结构查看代码组织。已有441人学习说明该主题在惯性姿态估计场景中具有一定关注度。读者可重点比较EKF与ESKF两条IMU处理路径理解双阶段滤波在陀螺仪、加速度计与磁力计融合中的差异并据此搭建复现、验证和排错思路。1. 线性假设失效后EKF 补的是什么扩展卡尔曼滤波EKF从根本上说并不直接解决非线性问题。它做的是在每个时间步把非线性函数在当前估计点附近展开为一阶近似直线再套用线性卡尔曼滤波的闭式解。这个说法解释了为什么一个“近似”的滤波器能在组合导航、目标跟踪、位姿估计等场合稳定使用多年。真正决定它能否正常工作的是雅可比矩阵在哪个点上求值、协方差被近似掉了多少、过程噪声矩阵 Q 到底在补偿哪部分偏差。搞懂这三件事调试 EKF 就会从依赖经验变成可验证的工程。2. 一阶泰勒展开雅可比矩阵为什么是 EKF 的全部秘密2.1 从非线性函数到局部线性模型线性卡尔曼滤波的推导可以归结为两个假设状态转移和观测都是线性的所有噪声和状态都服从高斯分布。高斯分布经过线性变换后仍是高斯分布因此整条推理链只需要均值和协方差两个量就能完整描述概率分布。滤波器的每一次预测和更新本质上是在做高斯分布的加法、乘法和线性映射。现实系统里这两条假设几乎都不成立。测距是平方根运算方位角是反正切姿态更新是旋转矩阵连乘位姿复合是流形上的运算。这些函数一旦套进线性卡尔曼滤波的框架高斯假设立刻失效一个高斯分布经过非线性变换后分布形状可能偏移可能形成双峰也可能在某个方向上被拉长成弯曲的形状。EKF 的处理方式是在每个时刻围绕当前估计点 $\hat{x}$ 对非线性函数做一阶泰勒展开并把二次及以上的项丢掉。$$ f(x) \approx f(\hat{x}) F,(x - \hat{x}), \qquad F \left.\frac{\partial f}{\partial x}\right|_{\hat{x}} $$这个操作相当于用一个超平面去贴近一张曲面贴近点就是当前估计。距离越远误差越大。丢掉二阶项之后原本非高斯的分布被强行恢复为高斯EKF 输出的协方差描述的是一个被扭曲过的分布。这正是“有偏”的来源也是 EKF 只能称为“近似最优”的原因。这里有一个很容易被忽略的细节线性化所依据的点是“估计值”而不是“真值”因为真值永远未知。因此 $F$ 和 $H$ 每一帧都在变化它们是随状态变化的矩阵而不是常数矩阵。很多初次实现 EKF 的代码把雅可比算成常量矩阵在小误差范围内似乎还能运行一旦状态变化稍微剧烈滤波器就会发散。常见的实现思路是把非线性函数就地线性化再借用线性最优增益公式。这种“就地”两个字是 EKF 全部价值的来源也是它全部问题的来源。2.2 雅可比矩阵的演练与验证方式用一个最小的例子把推导走一遍。状态取四维 $[p_x, p_y, v_x, v_y]$观测只有雷达的距离和方位$$ h(x) \begin{bmatrix} \sqrt{p_x^2 p_y^2} \ \operatorname{atan2}(p_y, p_x) \end{bmatrix} $$对 $h$ 求偏导得到 $2 \times 4$ 的观测雅可比 $H$。距离对位置的两个偏导分别是 $p_x/r$ 和 $p_y/r$它们正好是单位视线向量的两个分量方位角的两个偏导是 $-p_y/r^2$ 和 $p_x/r^2$。速度分量对应的两列为零因为观测与速度无关。这种几何解释很有用它能帮你判断符号是否写反因为单位视线向量一定指向目标。手推偏导是 EKF 实现里最容易出错的一步一个负号写错滤波器不会报错只会缓慢地往错误方向收敛。比较可靠的做法是用数值差分做对拍import numpy as np def h(x): 观测函数距离 方位 px, py x[0], x[1] return np.array([np.hypot(px, py), np.arctan2(py, px)]) def H_analytic(x): 解析雅可比2x4 px, py x[0], x[1] r2 px * px py * py r np.sqrt(r2) return np.array([[px / r, py / r, 0.0, 0.0], [-py / r2, px / r2, 0.0, 0.0]]) def H_numeric(x, eps1e-6): 中心差分用来对拍解析雅可比 n x.shape[0] out np.zeros((2, n)) for i in range(n): d np.zeros(n) d[i] eps out[:, i] (h(x d) - h(x - d)) / (2.0 * eps) return out x np.array([3.0, 4.0, 0.5, 0.2]) err np.abs(H_analytic(x) - H_numeric(x)).max() print(err) # 正常应在 1e-8 量级大于 1e-4 说明偏导推错了eps取 $10^{-6}$ 是中心差分的常规选择精度是 $O(\varepsilon^2)$再小会被浮点误差反过来掩盖。这段对拍的真正价值在于它在一帧之内就能暴露出符号错误和参考系错误。方位角那一行的差分要特别注意如果 $x$ 的取值让atan2跨过 $\pm\pi$ 边界数值差分会算出接近 $2\pi$ 的差值对拍结果会异常大但错误出在差分本身而不是解析解。提示手推完雅可比之后先用随机 generated 的几组状态做数值对拍再接入滤波器。这一步花的时间远比事后查发散便宜。2.3 线性化点先验、后验与迭代重线性化EKF 在两个地方需要雅可比两处用的线性化点不同。预测步用的是上一步的后验估计 $\hat{x}{k-1|k-1}$因为那是已知的、同时也是最接近真实状态的可用值。观测更新步用的是本步的先验 $\hat{x}{k|k-1}$因为此时还没有后验只能在先验上求 $H$。这个顺序带来了一个隐藏限制观测雅可比在预测点上求值如果预测点本身离真值很远$H$ 就没有参考价值。初始几帧、状态发生突变、传感器突然收到大范围观测时EKF 都会遇到这个问题表现出收敛缓慢、输出跳变严重时协方差更新方向相反直接发散。迭代扩展卡尔曼滤波IEKF就是针对这个缺陷提出的在先验点上线性化做一次更新得到后验再用后验重新线性化再更新循环若干次直到状态增量足够小。从优化视角看这等价于对最大后验目标函数做高斯牛顿迭代每一次重线性化都在降低线性化误差。代价是每帧的计算量成倍增加实时性要求高、非线性温和的场景没有必要采用。3. 协方差怎么传播P 矩阵在非线性下的近似代价3.1 P 的传播与 Q 的真实角色线性情形下协方差传播公式 $P^- F P F^{\mathsf{T}} Q$ 是精确成立的它来自高斯分布经过线性变换后的方差公式。非线性情形下这个式子变成了一阶近似真正的协方差还有一项与函数二阶导Hessian相关的修正量级正比于 $\lVert P \rVert$。协方差越大二阶项越不能忽略这也是为什么 EKF 在“不确定性很大”的时候特别不可靠。$Q$ 的含义经常被误解。教科书把它叫“过程噪声协方差”于是很多人把它当成传感器或执行机构的随机噪声去标定。实际工程里$Q$ 很大一部分在补偿的是“模型没有建模到的部分”没考虑的风阻没建模的轮胎滑移被简化掉的姿态动力学。$Q$ 设得太小滤波器会过度相信预测新观测的信息被增益 $K$ 压缩掉输出的协方差也偏小形成“过度自信”一旦真实状态偏离模型滤波器会拒绝相信观测最终发散。$Q$ 设得太大滤波器会过度相信观测输出抖动、滞后减小但噪声被放大。常见的实现思路是给 $Q$ 按物理量纲分解位置通道上给与加速度扰动相匹配的量级速度通道上给与力扰动相匹配的量级再乘上离散化时间。这样即使数值要调也是在一个有物理意义的区间里调而不是全局乱试。3.2 观测更新S、K 与马氏距离观测更新的三个核心量是新息 $\nu z - h(\hat{x}^-)$、新息协方差 $S H P^- H^{\mathsf{T}} R$、增益 $K P^- H^{\mathsf{T}} S^{-1}$。$K$ 的几何意义很清楚它是在“相信预测”与“相信观测”之间按协方差做的加权。$S$ 越小说明这个观测方向上预测的不确定性相对观测噪声更小增益越大反过来$R$ 大则 $K$ 小。$S$ 还有一个被低估的用途一致性检验。归一化新息平方 $\text{NIS} \nu^{\mathsf{T}} S^{-1} \nu$ 在滤波器一致时应该服从自由度等于观测维数的卡方分布。这条性质比任何曲线观察都客观它能把“滤波器是不是过度自信”变成一个可以画出来的统计量。R的标定相对容易把设备静止放置采集一段观测算样本协方差即可。难点在于角度通道静止时方位角读数在真值附近抖动直接算方差没问题但如果真值恰好落在 $\pm\pi$ 附近样本会横跨边界方差会被算成接近 $\pi^2$。处理方法是先把角度展开到连续值再统计。3.3 数值稳定性约瑟夫形式与正定性朴素更新式 $P (I - K H) P^-$ 在数学上等价于精确的贝叶斯更新但在浮点运算里会出问题$I - K H$ 不是正交矩阵乘积会让 $P$ 逐渐失去对称性长时间运行后可能出现负特征值滤波器要么输出 NaN要么在某个方向上“负方差”行为完全失控。约瑟夫形式Joseph form解决的就是这个$$ P (I - K H) P^- (I - K H)^{\mathsf{T}} K R K^{\mathsf{T}} $$def update(P, H, R, K): 约瑟夫形式协方差更新对任意 K 都保持半正定 n P.shape[0] I np.eye(n) A I - K H # 对称化抵消浮点累积造成的微小不对称 P_new A P A.T K R K.T return 0.5 * (P_new P_new.T)这段代码有两个值得说明的地方。第一$A P A^{\mathsf{T}} K R K^{\mathsf{T}}$ 的结构保证了两项都是半正定的只要 $P$ 和 $R$ 半正定结果就半正定与 $K$ 是否取最优值无关。第二末尾的对称化不是形式主义矩阵乘法本身会引入 $10^{-16}$ 量级的非对称几百帧累积下来协方差求逆时会出现方向性偏差。注意约瑟夫形式只解决数值层面的正定性不解决线性化误差导致的理论不一致。滤波器显示协方差很小、实际误差很大属于一致性问题需要靠 NIS 检验发现。4. 从公式到可跑代码一份 EKF 的骨架实现4.1 预测步f 与 F 的分工把二维匀速模型配雷达观测作为骨架。状态 $[p_x, p_y, v_x, v_y]$采样周期 $dt$过程噪声通过加速度扰动 $q$ 注入。import numpy as np def f_predict(x, dt): 状态转移匀速模型 px, py, vx, vy x return np.array([px vx * dt, py vy * dt, vx, vy]) def F_jacobian(dt): 匀速模型的雅可比是常数矩阵这是特例不是通例 return np.array([[1.0, 0.0, dt, 0.0], [0.0, 1.0, 0.0, dt ], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) def Q_matrix(dt, q): 把加速度扰动 q 映射到状态空间位置项含 dt^3/3 dt2 dt * dt dt3 dt2 * dt return q * np.array([[dt3 / 3, 0, dt2 / 2, 0 ], [0, dt3 / 3, 0, dt2 / 2], [dt2 / 2, 0, dt, 0 ], [0, dt2 / 2, 0, dt ]])Q_matrix里位置项是 $dt^3/3$、交叉项是 $dt^2/2$这个系数来自连续白噪声加速度模型对时间积分后的协方差不是随便填的。如果用 $q \cdot I$ 这种简化形式位置和速度通道的噪声比例就没法分别调节dt变化时滤波器行为也会改变。q的量纲是加速度方差物理意义明确调起来有参照。匀速模型的 $F$ 是常数矩阵这只是因为模型本身线性。一旦换成转弯模型、自行车模型或带姿态的状态$F$ 就会依赖当前状态必须在每帧重新计算。4.2 更新步新息、增益与角度归一化def ekf_update(x_pred, P_pred, z, R): z [距离, 方位]方位必须与预测方位做角度差归一化 px, py x_pred[0], x_pred[1] r np.hypot(px, py) r2 r * r # 观测预测 z_pred np.array([r, np.arctan2(py, px)]) # 新息角度通道必须先 wrap 到 [-pi, pi]否则 pi 与 -pi 的差是 2*pi nu z - z_pred nu[1] (nu[1] np.pi) % (2.0 * np.pi) - np.pi # 观测雅可比 H np.array([[px / r, py / r, 0.0, 0.0], [-py / r2, px / r2, 0.0, 0.0]]) S H P_pred H.T R K P_pred H.T np.linalg.inv(S) x_new x_pred K nu # 状态里的角度也要归一化避免累积超过 [-pi, pi] x_new[1] (x_new[1] np.pi) % (2.0 * np.pi) - np.pi I np.eye(4) A I - K H P_new A P_pred A.T K R K.T P_new 0.5 * (P_new P_new.T) return x_new, P_new, nu, Snu[1]那一行是整个更新步里最关键的一行。距离新息可以直接相减角度新息不行观测方位是 $-179°$、预测方位是 $179°$ 时直接相减得到 $-358°$滤波器会认为误差接近一整圈然后用一个巨大的增益把状态推向错误方向。归一化之后差值变成 $2°$才是真实的角度偏差。这个 bug 的隐蔽性在于它只在目标靠近 $\pm\pi$ 边界时触发跑直线轨迹的仿真里根本复现不出来。np.linalg.inv(S)在小维度上没问题观测维度升到十几维时应该改用 Cholesky 分解求解 $K P H^{\mathsf{T}} S^{-1}$既快又稳还能顺便拿到 $S$ 的正定性检查。4.3 用 NEES 与 NIS 检验滤波器是否“说真话”调参最怕的是看着轨迹曲线觉得“还行”。曲线能看出滞后和抖动看不出滤波器是不是在骗自己。一致性检验提供的是统计层面的判据滤波器报告的协方差是否真实反映了误差分布。状态误差平方的马氏范数 $\text{NEES} (x - \hat{x})^{\mathsf{T}} P^{-1} (x - \hat{x})$ 在一致时应服从自由度等于状态维数的卡方分布$\text{NIS} \nu^{\mathsf{T}} S^{-1} \nu$ 服从自由度等于观测维数的卡方分布。仿真里真值已知NEES 可以直接算实车上真值未知只能用 NIS。from scipy.stats import chi2 def nees_ok(nees_series, n): Monte Carlo 平均后的 NEES 落在双侧 95% 区间内即认为一致 mean_nees np.mean(nees_series) lo chi2.ppf(0.025, n * len(nees_series)) / len(nees_series) hi chi2.ppf(0.975, n * len(nees_series)) / len(nees_series) return lo, mean_nees, hi判断标准很直白平均 NEES 显著大于上界说明滤波器过度自信报告的 $P$ 比实际误差小需要增大 $Q$ 或 $R$显著小于下界说明滤波器过度保守协方差虚高跟踪会显得迟钝。这条判据比反复试参数可靠得多也是把“调参”变成“诊断”的转折点。5. 避坑与排查EKF 发散的五个典型现场5.1 现象航向角在 $\pm\pi$ 附近突然跳变滤波器崩掉原因几乎一定是新息没有做角度归一化。角度是周期量$179°$ 和 $-179°$ 在物理上只差 $2°$在数值上差了 $358°$。只要轨迹或目标穿越这条边界直接相减得到的新息就会被放大成一个大误差增益随之被推到接近 1状态被强行拉向错误一侧。解决方式是所有涉及角度的减法都包一层归一化(a - b pi) % (2*pi) - pi。同一套规则要贯穿新息计算、状态更新后的角度修正、以及 $R$ 矩阵的角度通道标定。血泪经验是这条规则必须写成工具函数全局复用而不是在每个调用点手写一遍。5.2 现象观测更新后状态往反方向走越更新越偏原因是观测雅可比的符号或者参考系约定与观测模型不一致。典型场景是方位角定义为“从 x 轴逆时针”还是“从 y 轴顺时针”以及旋转矩阵是主动旋转还是被动旋转。这两套约定在单看公式时都自洽混用之后偏导符号就反了。排查方法是用数值差分对拍对同一个状态点比较解析雅可比和差分结果误差超过 $10^{-4}$ 就说明推导有问题。更彻底的做法是先用一个已知真值、无噪声的仿真跑单帧更新看估计值是否向真值靠近如果一帧之后就偏离问题一定在雅可比而不是在噪声参数。5.3 现象轨迹平滑但严重滞后或者抖动剧烈这是 $Q$ 和 $R$ 的比例失衡。$Q$ 相对 $R$ 太小滤波器过度相信模型观测被当成噪声压掉输出表现为滞后$Q$ 相对 $R$ 太大滤波器追着观测跑输出表现为抖动。关键认识是这两个矩阵只有相对比例重要同时放大或缩小不改变增益。所以正确的调参顺序是先按物理量纲把 $R$ 从静止数据标出来定死再调 $Q$。如果反过来先调 $R$就会陷入两个参数互相抵消的死循环。判据用 NIS平均 NIS 高于卡方上界就增大 $Q$低于下界就减小 $Q$。5.4 现象跑了几千帧后 P 出现负特征值输出 NaN协方差在浮点运算中失去正定性。常见诱因有三个用朴素形式做更新、用单精度浮点、状态量纲差异过大导致矩阵条件数很差。解决分三层。第一层换成约瑟夫形式更新并且每次更新后做对称化。第二层做量纲归一化把位置用米、速度用米每秒统一到同一数量级避免 $10^6$ 和 $10^{-6}$ 出现在同一个矩阵里。第三层如果还不行改用平方根滤波直接传播协方差的 Cholesky 因子从结构上保证正定性代价是实现复杂度上升一档。5.5 现象初始几帧怎么都不收敛或者大范围观测时突然发散线性化点离真值太远一阶泰勒展开的截断误差已经大到让 $H$ 失去意义。这种情形在初始状态猜得很差、或者观测突然跳到大范围内时最明显。处理方式按成本从低到高排给一个更好的初值并且把初始协方差 $P_0$ 设得足够大让滤波器在早期愿意相信观测对前若干帧做迭代重线性化用更新后的估计重新计算 $H$如果非线性程度本身就很高大视场方位观测、强机动目标那就该考虑换滤波器而不是继续调参。6. 设计取舍为什么明知有偏还要用 EKF6.1 三种替代方案各自的代价EKF 的一阶近似有明确缺陷学术界也早就给出了更好的方案但工程上 EKF 依然是主力原因在于代价结构。方案精度单帧计算量主要代价EKF一阶$O(n^3)$一次矩阵求逆需要雅可比强非线性下有偏UKF二阶$O(n^3)$$2n1$ 个 sigma 点传播需 Cholesky高维时点数膨胀粒子滤波任意分布$O(N \cdot n^2)$$N$ 通常上千维度灾难实时性差因子图优化全局最优随窗口增长需要回环与重线性化延迟高UKF 用一组确定性采样点绕过雅可比能捕获二阶项实现上不需要手推偏导这一点在大状态维度下非常诱人。但 sigma 点数量是 $2n1$状态到二三十维时计算量就超过 EKF 了而且它对协方差的 Cholesky 分解很敏感$P$ 一旦不正定就直接崩比 EKF 更娇气。粒子滤波能表达多峰分布代价是高维下需要指数级的粒子数。因子图优化能给全局最优解代价是延迟和内存适合离线或低频后端不适合每毫秒都要出结果的场合。6.2 我实际判断的顺序接手一个估计问题时判断顺序大致是这样先看非线性程度。如果状态转移和观测在工作的取值范围内近似线性或者虽然非线性但协方差很小线性化误差正比于 $\lVert P \rVert$EKF 就是性价比最高的选择嵌入式上几千行代码就能跑协方差显式可读出问题能定位。如果非线性明显大角度旋转、大视场观测、强机动先试着用迭代重线性化把精度补回来成本比换 UKF 低。如果还是过不了 NEES 检验再换 UKF。只有在分布本身是多峰的数据关联不确定、遮挡导致观测断续时才值得上粒子滤波或多假设方法。有一条判断标准我一直在用能不能用 NIS 把滤波器的一致性量化出来。能就说明这套 EKF 是“可观测、可诊断”的值得投入调参不能比如观测维度太低、或者数据里根本没有激励那再怎么调都是在盲猜不如先去改传感器配置或观测模型。从那以后我每次接手一个 EKF 工程第一件事不是调 Q 和 R而是先跑一遍 NIS 一致性检验把滤波器是不是在“骗自己”变成一条有上下界的曲线摆在面前再去动参数。这个习惯让我少走了很多弯路也希望帮到你。本文还有配套的精品资源点击获取