1. 走上ESKF这条路之前：标准卡尔曼在IMU融合里的三个硬伤
先从一个我实际踩过的坑说起。好几年前我在做一个室内移动机器人的定位模块，硬件配置很简单：一个消费级IMU、一个低频UWB定位基站，期望输出20Hz左右的平滑位置。最开始图省事，直接用标准的扩展卡尔曼滤波去融合，状态量选了位置、速度、姿态四元数和两个零偏。跑起来之后发现一个特别诡异的现象：滤波跑着跑着，四元数的模长会慢慢偏离1，然后在某次更新之后姿态突然"跳变"一下，接着位置也跟着发散。当时我以为是代码里哪里忘了归一化，翻来覆去找了两天才意识到，这不是简单的工程失误，而是用四元数当状态向量本身就有问题。
这就是误差状态卡尔曼滤波（Error State Kalman Filter，ESKF）想要解决的核心问题。这套方法最早在惯性导航领域被大量使用，后来随着视觉惯导里程计（VIO）和组合导航系统的普及，又回到了机器人圈子的视野里。它不是什么全新的滤波理论，而是对EKF的一种改进用法：不是直接对完整状态做滤波，而是把状态拆成"名义状态"和"误差状态"，把标准卡尔曼滤波作用在误差上。你不需要懂很深的理论就能把ESKF用起来，但如果你不想在工程里踩一堆莫名其妙的坑，最好还是先搞清楚它到底解决了什么问题。
1.1 四元数维度和约束问题
先看姿态表示。一个三维刚体的姿态，自由度是3。用旋转矩阵表示需要9个数，用四元数表示需要4个数，但它们都有一个共同点：这些表示方式不是"自由向量"，而是带约束的。旋转矩阵要求正交且行列式为1，四元数要求模长为1。标准卡尔曼滤波的所有推导都假设状态是一个欧几里得向量空间的元素，你可以对状态做加减法、乘以矩阵、做高斯分布假设。可四元数不是这样的，你把两个四元数加起来，模长大概率就不是1了，你没法在四元数空间里直接定义一个有意义的高斯噪声。
你可能会说，那我滤波完之后做一次归一化不就行了？很多初学者的确这么干。但问题在于，卡尔曼滤波器内部的协方差矩阵表达的是"状态估计值的不确定性"，如果你强行把状态投影回约束流形上，这个投影操作会对协方差产生什么影响，你完全不知道。归一化这一步破坏了滤波器对不确定性的描述，短期看没事，长期跑下来协方差和实际误差就会越来越不匹配，最终导致滤波发散。
ESKF对这个问题处理得非常优雅：真实状态被拆成名义状态和误差状态，名义状态在流形上运动，负责"承接"非线性动力学，而误差状态是一个小量，可以安全地当作平面向量来处理。
1.2 线性化精度问题
标准EKF的线性化点是当前状态估计值。如果状态估计值和真值之间误差很大，那么围绕估计值做一阶泰勒展开得到的雅可比矩阵，并不能很好地代表系统在这一刻的真实动态。尤其在IMU积分这种强非线性系统里，姿态误差会通过旋转矩阵耦合进位置和速度的预测中，一个小的姿态误差在几秒内就可能被放大成很大的位置漂移。
ESKF的思路是把大误差留给名义状态的非线性积分去处理，误差状态几乎总是保持在一个很小的邻域内，因此对误差状态做线性近似，精度远高于对完整状态做线性近似。用弗拉基米尔·贝洛维奇经常说的一句话就是：误差状态的线性化误差，比真实状态的线性化误差小一个数量级。
1.3 可观测性和退化问题
还有一个工程上的实际问题:直接对完整状态做EKF时，状态向量通常包含位置、速度、姿态、陀螺仪零偏、加速度计零偏，这一共16维。但很多场景下，你手里的观测根本不足以同时把这些量都约束住。比如室内只有位置观测的时候，姿态和零偏的可观测性就很弱。
ESKF并没有从数学上改变系统的可观测性，但它给了你一个非常自然的工具去控制这种情况——你可以对待估计的误差状态做降维处理，只对当前可观测的误差状态做更新，其余部分继续靠名义状态积分去推。这种灵活性在实际工程里非常有用，我后面会细说。
2. ESKF的核心思想：把状态拆成"大体量"和"小误差"两半
ESKF的数学框架看起来有点绕，但本质上是这样一个关系：
x_true = x_nominal ⊕ x_error
这里⊕表示流形上的复合运算。以惯性导航为例，真实状态由以下部分组成：
- 位置 p（三维）
- 速度 v（三维）
- 姿态 q（四元数）
- 加速度计零偏 b_a（三维）
- 陀螺仪零偏 b_g（三维）
名义状态同样包含这五项，它们的区别在于：名义状态完全由IMU测量积分而来，不考虑测量噪声和零偏的细节影响，只是用当前的零偏估计值去补偿IMU原始测量。误差状态则用来表达"名义状态和真实状态之间的差距"。
2.1 真实状态、名义状态、误差状态的定义
用符号来写就是这样：
- 位置误差：δp = p_true - p_nom
- 速度误差：δv = v_true - v_nom
- 姿态误差：δθ = log(q_nom^{-1} ⊗ q_true)，这是一个三维旋转向量
- 零偏误差：δb_a = b_a_true - b_a_nom，δb_g = b_g_true - b_g_nom
这里最关键的是姿态误差。它被定义为名义姿态到真实姿态之间的旋转向量，是一个三维量。这样，整个误差状态向量就是15维的：
δx = [δp, δv, δθ, δb_a, δb_g] ∈ R^15
你注意到没有，误差状态是普通向量，它对加减法封闭，可以放心地用高斯分布去描述它的不确定性。这是ESKF整个算法能够成立的基石。
2.2 为什么误差状态可以安全地线性化
误差状态的运动学方程推导出来后，会呈现一个非常漂亮的线性形式。原因在于：当δθ是一个小量时，旋转矩阵可以展开为一阶近似。如果你去看误差状态的连续时间方程，会发现它只在一两个地方出现状态的乘积，而且那些乘积都是小量乘积，可以直接忽略。这意味着预测方程可以写成：
δx_dot = F_c * δx + w
其中F_c是一个15x15的矩阵，w是噪声项。这正是卡尔曼滤波最经典的形式，不需要像EKF那样在每一时刻重新计算非线性函数的雅可比矩阵。即使要算，也只是一个简单的矩阵，推导一次就能固定下来。
2.3 误差状态的注入与重置
每次滤波更新完成后，误差状态会被"注入"回名义状态，这一步叫reset。操作是把名义状态和误差状态重新复合，然后把误差状态置为零，同时更新协方差矩阵。
注意，这个重置操作不是简单地置零，它会导致协方差矩阵产生一个变换，因为名义状态变了，误差状态的原点也变了。如果不做这一步协方差调整，滤波器同样会出问题。这个细节在教科书里经常被一笔带过，但在工程实现中是个重要的坑，我后面会专门讲。
这个过程其实很像一个反馈控制器：名义状态是前馈积分，误差状态是反馈校正，卡尔曼滤波负责决定这个反馈强度。
3. 从连续时间到离散化：ESKF的完整推导过程
先明确我们讨论的是最经典的IMU+外部观测场景。IMU提供加速度计和陀螺仪的原始测量，用来做状态预测；外部观测（GNSS、UWB、视觉位姿等）用来对预测结果进行修正。
3.1 IMU测量模型和误差状态运动学
IMU的测量模型可以写成：
- 加速度计测量：a_m = R^T (a - g) + b_a + n_a
- 陀螺仪测量：ω_m = ω + b_g + n_g
其中a是物体在世界系下的真实加速度，g是重力向量，R是当前姿态，b_a和b_g是零偏，n_a和n_g是测量白噪声。
名义状态的连续时间方程是：
- ṗ_nom = v_nom
- v̇_nom = R_nom * (a_m - b_a_nom) + g
- q̇_nom = q_nom ⊗ [0, ω_m - b_g_nom] / 2
- ḃ_a_nom = 0
- ḃ_g_nom = 0
这里名义状态认为零偏不变，全部由滤波更新去修正。
误差状态的连续时间方程可以通过对真实状态和名义状态做差分推导出来。这里跳过复杂的推导过程，直接给出在机器人领域最常用的形式。定义：
- F_c矩阵中用到两个3x3反对称矩阵：[a]×表示a的反对称矩阵
误差状态方程：
δṗ = δv δv̇ = -R_nom * [a_m - b_a_nom]× * δθ - R_nom * δb_a - R_nom * n_a δθ̇ = -[ω_m - b_g_nom]× * δθ - δb_g - n_g δḃ_a = n_ba δḃ_g = n_bg这个方程组就是整个ESKF预测部分的核心。它已经是一个线性方程组，矩阵F_c的形式是：
F_c = [ 0 I 0 0 0 ] [ 0 0 -R*[a]× -R 0 ] [ 0 0 -[ω]× 0 -I ] [ 0 0 0 0 0 ] [ 0 0 0 0 0 ]其中a = a_m - b_a_nom，ω = ω_m - b_g_nom。这个矩阵的稀疏性非常好，在实际实现中可以有两种选择：直接用稀疏矩阵乘法，或者把这个15x15矩阵分块乘到9维的核心状态上。考虑到GPS/视觉融合场景下的实时性要求，建议直接按分块乘写，省去不必要的零矩阵乘法。
3.2 离散化：用中值法处理角速度积分
连续时间方程要落地到代码里，必须离散化。最常用的做法是中值法：用当前时刻和上一时刻的IMU测量平均作为整体时间段内的等效测量值。这样比简单的欧拉法精度高不少，而且代码量增加很小。
对于状态转移矩阵，可以直接用一阶近似：
F_d = I + F_c * Δt
二阶近似：
F_d = I + F_c * Δt + 0.5 * F_c^2 * Δt^2
从实际效果看，在IMU频率为100-200Hz、单步时间5-10毫秒的情况下，一阶近似已经足够。只有在IMU频率低于50Hz或者运动特别剧烈时才需要二阶近似。我一般默认用一阶近似，把算力留给更重要的协方差更新。
离散化后的误差状态预测方程变成：
δx_k+1 = F_d * δx_k + w_k协方差更新：
P_k+1 = F_d * P_k * F_d^T + Q_dQ_d是离散化的过程噪声协方差。根据连续时间噪声的功率谱密度，可以用如下近似：
Q_d ≈ F_d * G_c * Q_c * G_c^T * F_d^T * Δt
其中G_c是噪声输入矩阵，Q_c是连续时间的噪声功率谱密度矩阵。
3.3 预测协方差的更新
过程噪声Q_d的构造需要特别注意。常用的方法是把IMU测量噪声和零偏随机游走分成两部分：
Q_d = Q_meas + Q_biasQ_meas来源于加速度计和陀螺仪的测量白噪声，Q_bias来源于零偏的随机游走。在实际工程中，测量噪声和零偏随机游走的方差数值可能差好几个数量级，这会让协方差矩阵P的条件数变得很大。一个实用的做法是：在协方差更新时用double类型计算，并且每隔一段时间对P做一次对称化处理，防止数值误差破坏对称性。
4. 观测更新：怎么把GPS/视觉的测量"打进"误差状态
预测部分只依赖IMU，任何外部信息都通过更新方程进入系统。ESKF在更新方程上的优势在这里体现得很明显——因为误差状态是线性的，观测模型只需要关心"误差状态到测量残差"这一层线性关系，不需要对原状态做任何雅可比推导。
4.1 观测模型的一般形式
假设外部观测器和状态之间满足：
z = h(x_true) + v
我们把它分解成：
z = h(x_nom ⊕ δx) ≈ h(x_nom) + H * δx + v
于是测量残差为：
y = z - h(x_nom)
对应的观测矩阵H = ∂h / ∂δx | δx=0。这一步推导通常比直接EKF要简单，因为δx的维度低，而且h往往只在少数维度上依赖误差状态。
4.2 位置和速度观测的具体雅可比
最常见的观测是GNSS/UWB的位置观测。这种情况h(x) = p_true，因此：
z = p_nom + δp + v y = z - p_nom H = [I_3x3, 0, 0, 0, 0]就这么简单。如果你有速度观测（比如轮式里程计或视觉光流速度），H的第二块是I_3x3。
对于姿态观测（比如视觉定位输出四元数），情况稍微复杂。观测模型是：
z_q = q_true = q_nom ⊗ q(δθ)
残差可以通过计算z_q与q_nom的旋转差得到：
δz = log(z_q ⊗ q_nom^{-1})这里δz本身就是一个三维旋转向量，它直接就是δθ的一个含噪观测。所以H矩阵在第三块是I。
这个性质非常清爽：旋转残差天然就是误差状态的一部分，不需要额外推导复杂的雅可比，这是标准EKF做不到的。
4.3 更新后的状态合成与协方差处理
拿到H、y、观测噪声R后，标准卡尔曼更新公式直接套用：
K = P * H^T * (H * P * H^T + R)^{-1} δx = K * y P = (I - K * H) * P这里要注意，K计算使用的是预测协方差P，而P的单位是"误差状态的协方差"，这个点必须想清楚，因为它和标准EKF里P的含义不完全一样。
更新完的δx要注入名义状态：
- p_nom ← p_nom + δp
- v_nom ← v_nom + δv
- q_nom ← q_nom ⊗ q(δθ)
- b_a_nom ← b_a_nom + δb_a
- b_g_nom ← b_g_nom + δb_g
然后误差状态清零，协方差做一次reset变换：
G = I, G[3:6, 3:6] = I - [0.5 * δθ]× （实际上是根据姿态误差的注入方式确定） P ← G * P * G^T我见过的不少实现会直接跳过这一步，在协方差较大时这个近似会导致滤波性能下降。严谨的做法还是保留。
5. 工程实现骨架：从矩阵到能跑的C++代码
理论讲得再多，最终还是要落到代码。我在这里给一个精简但完整的ESKF核心实现框架，基于Eigen库，适用于GNSS+IMU融合场景。
5.1 核心数据结构和SO3运算
#include <Eigen/Dense> #include <Eigen/Geometry> struct ImuMeasurement { Eigen::Vector3d acc; Eigen::Vector3d gyro; double timestamp; }; struct ErrorState { Eigen::Vector3d dp; Eigen::Vector3d dv; Eigen::Vector3d dtheta; Eigen::Vector3d dba; Eigen::Vector3d dbg; }; class ESKF { public: // 名义状态 Eigen::Vector3d p_ = Eigen::Vector3d::Zero(); Eigen::Vector3d v_ = Eigen::Vector3d::Zero(); Eigen::Quaterniond q_ = Eigen::Quaterniond::Identity(); Eigen::Vector3d ba_ = Eigen::Vector3d::Zero(); Eigen::Vector3d bg_ = Eigen::Vector3d::Zero(); // 误差状态协方差 Eigen::Matrix<double, 15, 15> P_ = Eigen::Matrix<double, 15, 15>::Identity(); // 噪声参数应考虑从配置读取 double noise_acc_ = 0.01; // 加速度计噪声标准差 double noise_gyro_ = 0.001; // 陀螺仪噪声标准差 double noise_acc_bias_ = 0.001; // 加速度计零偏随机游走 double noise_gyro_bias_ = 0.001; // 陀螺仪零偏随机游走 };SO3的操作直接用Eigen的Quaterniond和AngleAxis就可以。我自己封装了一个小函数方便做旋转向量到四元数的转换：
static Eigen::Quaterniond Vec2Quat(const Eigen::Vector3d& vec) { double angle = vec.norm(); if (angle < 1e-12) return Eigen::Quaterniond::Identity(); Eigen::Vector3d axis = vec / angle; return Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)); }5.2 Predict函数的实现
Predict函数接收两个相邻IMU测量，用中值法预测名义状态并且更新误差状态协方差：
void Predict(const ImuMeasurement& imu_main, const ImuMeasurement& imu_prev) { double dt = imu_main.timestamp - imu_prev.timestamp; // 中值法 Eigen::Vector3d acc = 0.5 * (imu_main.acc + imu_prev.acc) - ba_; Eigen::Vector3d gyro = 0.5 * (imu_main.gyro + imu_prev.gyro) - bg_; // 名义状态预测 Eigen::Quaterniond dq = Vec2Quat(gyro * dt); q_ = (q_ * dq).normalized(); Eigen::Vector3d a_world = q_ * acc + Eigen::Vector3d(0, 0, -9.81); v_ += a_world * dt; p_ += v_ * dt; // 误差状态转移矩阵 Eigen::Matrix<double, 15, 15> F = Eigen::Matrix<double, 15, 15>::Identity(); Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity(); Eigen::Matrix3d R = q_.toRotationMatrix(); F.block<3, 3>(0, 3) = I3 * dt; F.block<3, 3>(3, 6) = -R * Skew(acc) * dt; F.block<3, 3>(3, 9) = -R * dt; F.block<3, 3>(6, 6) = -Skew(gyro) * dt; F.block<3, 3>(6, 12) = -I3 * dt; // 离散噪声协方差 Eigen::Matrix<double, 15, 15> Q = Eigen::Matrix<double, 15, 15>::Zero(); Q.block<3, 3>(3, 3) = R * (noise_acc_ * noise_acc_ * I3) * R.transpose() * dt * dt; Q.block<3, 3>(6, 6) = noise_gyro_ * noise_gyro_ * I3 * dt * dt; Q.block<3, 3>(9, 9) = noise_acc_bias_ * noise_acc_bias_ * I3 * dt; Q.block<3, 3>(12, 12) = noise_gyro_bias_ * noise_gyro_bias_ * I3 * dt; // 协方差更新 P_ = F * P_ * F.transpose() + Q; // 对称化 P_ = 0.5 * (P_ + P_.transpose()); }其中Skew函数用来构造反对称矩阵：
static Eigen::Matrix3d Skew(const Eigen::Vector3d& v) { Eigen::Matrix3d m; m << 0, -v.z(), v.y(), v.z(), 0, -v.x(), -v.y(), v.x(), 0; return m; }这里有一个值得注意的地方：位置和速度的初始值matters很大。如果初始位置/速度不准，协方差P的初始值应该设置对应的不确定度，不要直接设为零矩阵。零协方差会被卡尔曼增益公式放大成"位置观测完全不可信"的效果，导致滤波器一开始就剧烈调整，反而起不到平滑作用。
5.3 UpdateWithGNSS的实现
GNSS位置更新是最典型的场景：
bool UpdateWithGNSS(const Eigen::Vector3d& pos_gnss, double timestamp) { // 观测残差 Eigen::Vector3d y = pos_gnss - p_; // 观测矩阵 Eigen::Matrix<double, 3, 15> H; H.setZero(); H.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); // 观测噪声 Eigen::Matrix3d V = Eigen::Matrix3d::Identity() * gnss_noise_; // 卡尔曼增益 Eigen::Matrix<double, 15, 3> K; Eigen::Matrix<double, 3, 3> S = H * P_ * H.transpose() + V; K = P_ * H.transpose() * S.inverse(); // 误差状态更新 Eigen::Matrix<double, 15, 1> dx = K * y; ErrorState es; es.dp = dx.block<3, 1>(0, 0); es.dv = dx.block<3, 1>(3, 0); es.dtheta = dx.block<3, 1>(6, 0); es.dba = dx.block<3, 1>(9, 0); es.dbg = dx.block<3, 1>(12, 0); // 注入名义状态 p_ += es.dp; v_ += es.dv; q_ = (Vec2Quat(es.dtheta) * q_).normalized(); ba_ += es.dba; bg_ += es.dbg; // 协方差更新 Eigen::Matrix<double, 15, 15> I = Eigen::Matrix<double, 15, 15>::Identity(); P_ = (I - K * H) * P_; // 误差状态重置的协方差修正 Eigen::Matrix<double, 15, 15> G = Eigen::Matrix<double, 15, 15>::Identity(); G.block<3, 3>(6, 6) = I - Skew(0.5 * es.dtheta); P_ = G * P_ * G.transpose(); P_ = 0.5 * (P_ + P_.transpose()); return true; }这个实现看起来不长，但每一个小块都有明确的含义。如果你想把ESKF从GNSS融合改成视觉位姿融合，只需要改H和y两部分，其他代码几乎可以原样复用。这也是ESKF在工程上比标准EKF更受欢迎的原因之一：算法的骨架是稳定的，换传感器只需要换观测模型的适配层。
6. 调参和踩坑：我在实际项目里遇到的问题
6.1 零偏随机游走的噪声参数怎么定
ESKF里的过程噪声参数有一部分是IMU数据手册直接给的，比如加速度计测量噪声的功率谱密度。但零偏随机游走这一项，很多IMU数据手册给的是"零偏稳定性"（单位是deg/h），需要换算成随机游走的功率谱密度。一个常见的工程做法是把数据手册给的零偏稳定性数值除以sqrt(3600)，近似当作离散的随机游走标准差。
但这里有个非常实际的问题：这个值通常只是近似。我遇到过好几次，同一个IMU型号在不同批次的产品上，随机游走参数能差3倍。如果你的滤波器对零偏收敛特别敏感，最可靠的办法是采集一段静止或匀速运动的数据，用Allan方差分析工具（比如imu_utils或pyallan）去拟合出实际的噪声参数。
6.2 协方差矩阵的对称性：一个小操作省好多bug
这看起来像是一个小问题，但实际影响非常大。卡尔曼滤波的协方差更新公式P = (I - KH)P，在理想代数下是严格对称的。但在浮点运算下，经过几十上百次迭代，非对称项会逐渐累积，最终可能导致下三角矩阵里的某个元素变成负数，然后卡尔曼增益计算出负的方差，滤波器直接崩掉。我在代码里会在每次更新P之后做一次对称化处理：
P_ = 0.5 * (P_ + P_.transpose());这个操作成本几乎可以忽略，但它能保证协方差矩阵一直保持数学上的合法性。这不是炫技，纯粹是工程上的保命操作。
6.3 误差状态清零之后别忘了重置P的耦合项
在大多数ESKF实现中，注入操作完成后，误差状态变量会被清零。但这里有一个很多人会忽略的点：注入操作改变了名义状态，而名义状态是误差状态线性化的参考点。虽然误差状态本身被清零了，但P矩阵里那些表示"误差状态之间相关性"的项，在新的参考点下的数值应该和旧参考点下的数值不同。
为了修正这一点，常规做法是引入一个雅可比矩阵G，它描述了误差状态在新旧参考点之间的变换关系。对于姿态误差的注入，G.block<3, 3>(6, 6) = I - Skew(0.5 * δθ)这个近似在δθ较小时精度足够。如果δθ很大（比如超过10度），更好的做法是用完整的SO(3)左雅可比矩阵。我一般会监控δθ的范数，如果发现每次更新的角度偏差超过一定阈值，就该怀疑是不是观测噪声参数设置过大了。
6.4 初值和坐标系对齐的影响
ESKF不像EKF那样直接对初始状态求逆，但它依然对初始状态很敏感。初始姿态误差如果大于30度，误差状态的线性化假设就失效了，滤波器在刚开始的几十步里可能输出震荡的结果。我通常会在正式滤波开始前，利用静止时刻的加速度计读数初始化俯仰和横滚角（利用重力方向），再用磁力计或外部观测初始化航向角。这一步看似简单，但从根上避免了系统在大初始误差下挣扎的情况。
7. 什么时候选ESKF，什么时候别选：方案取舍参考
ESKF并不是万能药。我在实际项目中总结了一些选型经验，可以作为参考。
ESKF非常合适用来处理IMU和外部低频传感器（GNSS、UWB、视觉里程计）融合。状态预测频率和更新频率解耦，这是它的天然优势——你可以在200Hz上预测，在10Hz上更新，并且两者的代码逻辑完全独立。同时它的计算量比UKF和粒子滤波低一个量级，在嵌入式平台上跑起来毫无压力。
如果你的系统几乎没有外部更新，全靠IMU推导，那ESKF帮助也不大。这种情况下误差状态的协方差会持续增长，滤波器本身并不能阻止积分发散。这时候你应该考虑的是做零速修正（ZUPT）或者增加传感器约束。
如果你的系统是纯视觉SLAM，后端有回环检测和全局优化，那么ESKF通常只作为IMU的前端预测模块，最终的全局位姿估计还是用因子图优化来做。ESKF的价值在于提供一个高频、低延迟的里程计输出，以及给优化后端提供可靠的相对约束信息。
还有一类场景是纯姿态估计，比如无人机飞控的姿态解算。ESKF在这里显得有点重——Mahony互补滤波和它的变体，在姿态精度、计算开销、调参便利性上都更合适。ESKF的优势在于它同时估计位置、速度、姿态和零偏，如果你只需要姿态，用15维状态向量是杀鸡用牛刀。
如果你使用的IMU噪声特别大、运动特别剧烈，ESKF的线性化假设也会受到挑战。误差状态虽然是小量，但它不会自己保证是小量——卡尔曼增益如果太大，误差状态的一次更新就可能跳出一个很大的值，破坏线性化假设。解决办法是限制单次更新的最大角度变化，或者在更新前后检查误差状态范数，超阈值时缩减增益。这也是我在工程中会主动加的一道保护逻辑。
回到文章最开始那个UWB定位的项目。后来我把融合算法从标准EKF换成了ESKF，同一份数据、同样的噪声参数，定位轨迹的平滑程度和稳定性都提升了不止一个台阶。最关键的是，姿态和位置之间的耦合问题被彻底绕过去了，四元数归一化检查从这个项目里消失了。从那以后，凡是涉及IMU+外部观测的融合任务，我的默认方案就是ESKF，教科书里的误差状态运动学推导我到现在还能默写出来。这套方法的好用程度，用过的人都懂。
