1. 从“猜”到“算”为什么我们需要卡尔曼滤波如果你做过机器人、无人机或者任何需要融合传感器数据的项目大概率听过卡尔曼滤波这个名字。它听起来很高深一堆矩阵公式让人望而却步。但它的核心思想其实非常朴素如何在充满噪声的世界里做出最好的估计想象一下你在一个烟雾弥漫的房间里试图定位一个移动的小球。你手上有一个不太准的尺子传感器测量值有噪声还有一个基于物理规律推算小球位置的模型系统模型也有误差。尺子告诉你小球在A点但你知道它可能偏左或偏右你的模型推算小球应该在B点但你也知道模型简化了现实比如忽略了空气阻力。那么此时此刻小球最可能在哪里卡尔曼滤波就是解决这个问题的“数学裁判”。它不相信单一的测量也不完全信任理论模型而是动态地、定量地结合两者给出一个比任何单一信息来源都更靠谱的“最优估计”。这个“最优”是数学上可证明的指的是在最小均方误差意义下的最优。为什么这如此重要因为现实世界的传感器没有完美的。MPU6050读取的角速度有漂移GPS定位有米级的误差摄像头识别有像素抖动。如果我们直接用这些带噪声的数据做控制或决策系统就会像喝醉了一样摇晃甚至崩溃。卡尔曼滤波通过“滤波”本质上是在做“去噪”和“信息融合”让系统能“看得更清走得更稳”。在自动驾驶中它融合摄像头、雷达和IMU的数据来精准定位在无人机飞控中它融合加速度计和陀螺仪数据来估算姿态在金融领域它甚至可以用来估计隐藏的市场状态。可以说凡是需要对动态系统状态进行实时、最优估计的场景卡尔曼滤波几乎都是首选工具。接下来我们就抛开复杂的数学外壳先看看它的核心思路流程让你直观理解这个“裁判”是如何工作的。2. 卡尔曼滤波的五步“思考回路”一个完整的迭代周期卡尔曼滤波不是一个单一的公式而是一个循环迭代的算法流程。每一次迭代对应每一个新的传感器数据到来时刻它都严格遵循五个步骤。理解这五步就理解了KF的骨架。我们用一个经典的例子来贯穿说明估算一个在直线上运动的小车的位置和速度。假设我们有一个小车我们想知道它在每一时刻的位置和速度。我们有一个不太准的GPS只能测位置噪声大还有一个基于上一时刻状态和加速度推算的运动模型。2.1 第一步预测状态时间更新在收到新的传感器数据之前我们先基于已知的物理规律“猜”一下小车现在应该在哪。这利用了系统的状态空间模型。我们定义小车的状态向量为x [位置; 速度]。假设小车做近似匀速运动但存在未知的扰动如风、路面不平我们用加速度a来建模这个扰动。运动方程离散时间可以写为新位置 旧位置 旧速度 * 时间间隔 0.5 * 加速度 * 时间间隔²新速度 旧速度 加速度 * 时间间隔用矩阵表示这就是状态转移矩阵 F的作用x_pred F * x_est_prev B * u其中x_est_prev是上一时刻的最优估计。F是状态转移矩阵对于匀速模型F [[1, dt], [0, 1]]dt是时间间隔。B是控制输入矩阵u是控制量这里可以认为是加速度a。如果我们将加速度视为过程噪声的一部分这一项有时可省略或合并。这一步之后我们得到了一个先验状态估计x_pred也叫预测状态。它纯粹基于模型推算还没有用到当前时刻的任何测量信息。注意这里的模型矩阵F是你对系统动力学的理解。如果模型本身偏离实际比如小车其实在急加速你却用了匀速模型预测就会产生偏差。这是过程模型误差的来源。2.2 第二步预测不确定性协方差更新只预测状态不够我们还得知道这个预测有多“不确定”。在卡尔曼滤波中不确定性用协方差矩阵 P来表示。P矩阵对角线上的值代表各个状态变量位置、速度自身的不确定性方差非对角线上的值代表状态变量之间的相关不确定性协方差。预测不确定性同样遵循模型传播P_pred F * P_est_prev * F^T Q其中P_est_prev是上一时刻估计的不确定性。Q是过程噪声协方差矩阵。它是整个KF中非常关键且需要你调参的矩阵。Q矩阵的物理意义是什么它代表了你的过程模型由F描述有多不准确。包括所有未建模的动态、外部扰动等。比如你认为小车匀速F基于此但实际上路面有坡、有风这些因素造成的状态偏差都归入Q。Q设得越大说明你越不相信自己的模型滤波器会更快地相信测量值。2.3 第三步计算卡尔曼增益现在我们有了一个带不确定性的预测 (x_pred, P_pred)。同时传感器测量值z也到了它自带测量噪声R。卡尔曼增益K就是一个权重系数它决定了我们应该在多大程度上用测量值来修正预测值。计算公式是K P_pred * H^T * (H * P_pred * H^T R)^(-1)看起来复杂我们来拆解H是观测矩阵。它负责将状态空间映射到测量空间。比如我们的GPS只测量位置不直接测速度那么H [1, 0]。这意味着测量值z位置只与状态向量中的第一个元素位置相关。R是测量噪声协方差矩阵。它代表了传感器有多不准。比如GPS厂商可能给出精度是±3米标准差那么方差就是9平方米。R通常可以通过传感器标定或数据手册获得。(H * P_pred * H^T R)这一项计算的是“预测的测量值”的不确定性由状态不确定性P_pred经H映射而来加上“实际的测量值”的不确定性R。两者之和代表了“测量残差”预测测量 vs 实际测量的总不确定性。整个公式的含义是卡尔曼增益 K 正比于预测的不确定性 P_pred反比于测量残差的总不确定性。如果预测非常不确定P_pred很大而传感器很准R很小那么K就会很大滤波器会赋予测量值很高的权重。如果预测很准P_pred很小或者传感器噪声很大R很大那么K就会很小滤波器会更相信自己的预测。2.4 第四步用测量值更新状态状态更新这是融合发生的一步。我们用卡尔曼增益来调和预测和测量x_est x_pred K * (z - H * x_pred)这个公式极其优美(z - H * x_pred)被称为测量残差或新息。它是实际测量值与你预测应该测量到的值之间的差。如果预测完美且测量无噪这项应为0。K * (新息)就是基于卡尔曼增益计算出的修正量。x_est就是最终的最优估计它等于预测值加上一个经过“合理性”加权后的修正。2.5 第五步更新估计的不确定性在融合了新的测量信息后我们状态估计的不确定性也应该减小。更新公式为P_est (I - K * H) * P_pred其中I是单位矩阵。这个公式可以这样理解因为我们引入了测量信息它带有确定性的信息所以我们对状态的认知不确定性降低了。(I - K*H)这个因子就像一个“折扣”将先验不确定性P_pred降低到后验不确定性P_est。至此一个完整的卡尔曼滤波周期结束。我们将x_est和P_est作为当前时刻的最优输出并作为下一轮迭代的“上一时刻估计” (x_est_prev,P_est_prev)等待新的传感器数据到来循环往复。3. 当世界变弯曲扩展卡尔曼滤波的引入与核心挑战标准卡尔曼滤波KF有一个非常强的前提假设系统的动态模型和观测模型都必须是线性的。也就是说状态预测方程x_pred F * x_prev和观测方程z H * x必须是矩阵乘法这种线性形式。这在很多理想或简化场景下成立比如我们之前假设的匀速直线运动。但现实世界充满了非线性。例如无人机、机器人的姿态运动学涉及三角函数sin,cos。雷达跟踪中从距离、方位角到直角坐标的转换。任何涉及平方、指数、三角关系的物理过程。这时如果我们强行用线性模型去近似非线性系统滤波结果往往会迅速发散变得毫无用处。扩展卡尔曼滤波EKF就是为了解决非线性系统的状态估计问题而生的。它的核心思想非常“工程师化”局部线性化。EKF不再要求全局的线性它只要求在当前估计点附近系统可以近似为一个线性系统。具体做法就是使用泰勒展开式的一阶项来近似非线性函数。这就像在一个弯曲的山坡上我们只看脚下那一小片区域把它当作一个平面来处理。3.1 EKF与KF流程的异同五步中的变与不变EKF的整体流程框架与KF完全一致依然是“预测-更新”的五步循环。变化发生在涉及模型F和H的地方预测状态x_pred f(x_est_prev, u)。这里f是一个非线性的状态转移函数而不是KF中的线性矩阵F。例如对于姿态更新f会包含四元数乘法或欧拉角积分。预测不确定性P_pred F_j * P_est_prev * F_j^T Q。关键变化来了这里的F_j不再是常数矩阵而是非线性函数f在x_est_prev处的雅可比矩阵Jacobian。它代表了f在当前工作点附近的局部线性近似。计算卡尔曼增益K P_pred * H_j^T * (H_j * P_pred * H_j^T R)^(-1)。同样H_j是非线性观测函数h(x)在预测点x_pred处的雅可比矩阵。h(x)将状态映射到预测的测量值例如从位置、速度状态预测雷达应观测到的距离和角度。更新状态x_est x_pred K * (z - h(x_pred))。注意残差计算也使用了非线性观测函数h(x_pred)。更新不确定性P_est (I - K * H_j) * P_pred。形式不变。可以看到EKF用非线性函数f和h进行状态和观测的预测但在传播不确定性协方差P时严格使用了这两个非线性函数在当前点的雅可比矩阵F_j和H_j。这是EKF最核心也最容易出错的地方。3.2 雅可比矩阵EKF的“心脏”与主要计算负担雅可比矩阵是什么简单说它是一个多维函数的“导数矩阵”。对于一个将n维状态向量映射到m维向量的函数y g(x)其雅可比矩阵J是一个m x n的矩阵其中第i行第j列的元素J_ij ∂g_i / ∂x_j即函数第i个输出对状态第j个分量的偏导数。为什么必须用雅可比矩阵在KF中不确定性P一个高斯分布经过线性变换F后仍然是一个高斯分布其协方差变换公式严格成立。在非线性变换f下一个高斯分布经过变换后一般不再是高斯分布。EKF的做法是先对非线性函数进行一阶线性近似求雅可比然后假设这个近似后的线性变换对高斯分布仍然有效。这是一种“一阶近似”策略。计算雅可比矩阵的两种方式解析法手动或使用符号计算工具如Matlab的jacobian函数Python的SymPy库推导出偏导数的闭合形式表达式。这是最精确、运行时效率最高的方法但推导过程繁琐容易出错。# 一个简单例子假设状态 x [px, py, vx, vy]观测是距离r和方位角θ # 观测函数 h(x) [sqrt(px^2 py^2), atan2(py, px)] # 则雅可比矩阵 H_j 为 # H_j [[∂r/∂px, ∂r/∂py, ∂r/∂vx, ∂r/∂vy], # [∂θ/∂px, ∂θ/∂py, ∂θ/∂vx, ∂θ/∂vy]] # 计算可得 # ∂r/∂px px / r, ∂r/∂py py / r, ∂r/∂vx 0, ∂r/∂vy 0 # ∂θ/∂px -py / r^2, ∂θ/∂py px / r^2, ∂θ/∂vx 0, ∂θ/∂vy 0数值法使用有限差分法在运行时近似计算。例如对于函数g(x)∂g_i/∂x_j ≈ (g_i(xε*e_j) - g_i(x)) / ε其中e_j是第j个单位向量ε是一个很小的数如1e-7。这种方法实现简单无需推导但计算量大需要多次调用函数且精度和稳定性受ε选择影响。实操心得在工程实践中对于复杂的模型首次实现时可以使用数值法来验证结果的正确性同时编写解析法的代码。一旦验证无误应切换到解析法以保证实时性。很多开源导航算法库如ROS的robot_localization都要求用户提供雅可比矩阵的解析形式。4. EKF的“阿喀琉斯之踵”线性化误差与发散问题EKF通过一阶泰勒展开进行局部线性化这个策略虽然巧妙但也带来了固有的缺陷这些缺陷是你在使用EKF时必须时刻警惕的。4.1 线性化误差何时会“爆炸”一阶近似的误差与两个因素直接相关非线性程度函数f或h在估计点附近的曲率越大一阶近似忽略的高阶项二阶及以上的导数就越重要误差就越大。估计的不确定性协方差矩阵P的大小决定了你的“置信区间”范围。如果P很大即你对当前估计非常不确定那么状态可能分布在一个较大的区域。在这个大区域内非线性函数可能已经弯曲得非常厉害用一个点上的切线雅可比来代表整个区域的变换误差必然巨大。典型场景初始状态不确定滤波器刚开始运行时P通常初始化得很大。如果初始状态离真实值很远非线性函数在“错误”的点上线性化可能导致后续的估计完全跑偏无法收敛。剧烈机动对于跟踪问题当目标突然急转弯高机动时匀速或匀加速模型即使是非线性的会严重偏离实际导致预测误差剧增P变大进而放大线性化误差。观测几何差在某些观测角度下观测函数h(x)的非线性特性会变得非常强。例如用单目相机估计深度在物体距离很远时深度估计对像素坐标的变化极不敏感非线性强线性化效果很差。4.2 协方差矩阵的病态与数值不稳定EKF的协方差更新公式中涉及矩阵求逆(H_j * P_pred * H_j^T R)^(-1)。在以下情况下可能出问题观测信息不足例如在某些时刻传感器数据暂时失效或维度低于状态维度导致(H_j * P_pred * H_j^T R)矩阵接近奇异不可逆或条件数极大。求逆会数值不稳定卡尔曼增益K计算错误。P矩阵失去正定性理论上协方差矩阵P必须是对称正定矩阵代表方差为正。但由于数值计算舍入误差或者在推导雅可比矩阵时存在错误可能导致P更新后出现负的特征值即负方差这在物理上是不可能的。一旦P非正定后续计算将完全失控。4.3 应对策略从工程技巧到算法升级面对EKF的这些问题有一系列工程实践和高级算法可以应对1. 工程实践上的“止血”策略谨慎初始化尽可能提供准确的初始状态x0并将初始协方差P0设置为与你初始猜测的不确定性相匹配的合理值不要盲目设得过大。给过程噪声Q“上保险”当模型误差难以精确建模时适当调大Q矩阵。这相当于告诉滤波器“我的模型不太靠谱你多相信一点测量数据。”这有助于在系统发生未建模机动时让滤波器更快地跟上真实状态。但Q过大也会导致估计噪声变大。启用“健康监测”检查新息序列测量残差(z - h(x_pred))理论上应该是一个零均值的白噪声序列。你可以实时计算其均值和自相关如果发现明显偏离说明滤波器可能已经发散或者Q、R设置不当。强制P矩阵对称正定每次更新P后执行P (P P^T) / 2来保证对称性。对于更严格的场景可以使用乔里斯基分解并检查对角线元素的正负。应对数值问题使用更稳定的矩阵求逆算法如SVD分解或者在观测信息不足时跳过更新步骤只进行预测。2. 算法层面的升级无迹卡尔曼滤波当线性化误差成为主要矛盾时一个更强大的工具是无迹卡尔曼滤波。UKF采用了一种完全不同的思路它不再对非线性函数进行线性化而是采用“无迹变换”。核心思想精心选择一组具有代表性的样本点称为Sigma点这些点能精确捕获输入高斯分布的均值和协方差。将这些Sigma点直接通过非线性函数f和h进行传播然后从传播后的点集计算输出的均值和协方差。由于是直接处理非线性变换UKF能够捕获到二阶甚至更高阶的统计特性其精度通常优于EKF尤其是在非线性程度高的情况下。UKF不需要计算雅可比矩阵这省去了大量推导和编码工作也避免了因雅可比计算错误带来的bug。其代价是计算量略大于EKF需要传播2n1个Sigma点n为状态维度但对于现代处理器许多问题中这个开销是可以接受的。因此在条件允许时UKF通常是比EKF更推荐的选择。5. 从公式到代码一个完整的EKF实例解析理论说得再多不如一行代码。我们用一个经典的例子来实现一个EKF基于GPS位置和IMU加速度计数据融合估计车辆的位置、速度和姿态偏航角。这是一个简化的二维平面导航问题。状态定义x [px, py, vx, vy, yaw]^Tpx, py: 东向和北向位置米vx, vy: 东向和北向速度米/秒yaw: 偏航角从北向东旋转为正弧度传感器GPS提供px_gps, py_gps测量噪声较大。IMU加速度计提供车体坐标系下的纵向加速度a_x_imu和横向加速度a_y_imu并假设IMU可以提供一个相对准确的偏航角速度yaw_rate_imu。5.1 预测步骤的实现预测模型我们使用匀速转向模型。import numpy as np def predict(x, P, imu_data, dt, Q): 预测步骤 x: 上一时刻状态估计 [5,] P: 上一时刻协方差 [5,5] imu_data: 包含 a_x, a_y, yaw_rate dt: 时间间隔 Q: 过程噪声协方差矩阵 [5,5] px, py, vx, vy, yaw x a_x, a_y, yaw_rate imu_data[a_x], imu_data[a_y], imu_data[yaw_rate] # --- 1. 非线性状态预测 (f函数) --- # 注意IMU加速度是在车体坐标系需要转换到全局坐标系东北天 cos_yaw np.cos(yaw) sin_yaw np.sin(yaw) a_x_global a_x * cos_yaw - a_y * sin_yaw a_y_global a_x * sin_yaw a_y * cos_yaw # 状态预测 yaw_pred yaw yaw_rate * dt # 简单欧拉积分对于高频率数据或复杂运动需用更精确积分 vx_pred vx a_x_global * dt vy_pred vy a_y_global * dt px_pred px vx * dt 0.5 * a_x_global * dt**2 py_pred py vy * dt 0.5 * a_y_global * dt**2 x_pred np.array([px_pred, py_pred, vx_pred, vy_pred, yaw_pred]) # --- 2. 计算雅可比矩阵 F_j --- # 这是f函数对状态x的偏导数在x点处计算 F_j np.eye(5) # 先初始化为单位矩阵 # 位置对速度的偏导 F_j[0, 2] dt # ∂px/∂vx F_j[1, 3] dt # ∂py/∂vy # 位置对偏航角的偏导 (来自加速度转换) F_j[0, 4] (-a_x * sin_yaw - a_y * cos_yaw) * 0.5 * dt**2 F_j[1, 4] (a_x * cos_yaw - a_y * sin_yaw) * 0.5 * dt**2 # 速度对偏航角的偏导 F_j[2, 4] (-a_x * sin_yaw - a_y * cos_yaw) * dt F_j[3, 4] (a_x * cos_yaw - a_y * sin_yaw) * dt # 偏航角对自身和角速度的偏导这里假设yaw_rate是控制输入不放在状态里所以只对自身 F_j[4, 4] 1.0 # ∂yaw/∂yaw实际上模型里是 yaw_rate*dt对yaw求导为1 # --- 3. 预测协方差 --- P_pred F_j P F_j.T Q return x_pred, P_pred5.2 更新步骤的实现假设我们此时收到了GPS位置数据。def update_gps(x_pred, P_pred, gps_data, R_gps): GPS更新步骤 x_pred, P_pred: 预测的状态和协方差 gps_data: 包含 px_gps, py_gps R_gps: GPS测量噪声协方差 [2,2] px_gps, py_gps gps_data[px], gps_data[py] z np.array([px_gps, py_gps]) # --- 1. 非线性观测函数 h(x) 及其雅可比 H_j --- # GPS直接观测位置所以 h(x) [px, py] # 这是一个线性观测但为了保持EKF框架统一我们还是用雅可比 H_j np.zeros((2, 5)) H_j[0, 0] 1 # ∂(观测px)/∂(状态px) 1 H_j[1, 1] 1 # ∂(观测py)/∂(状态py) 1 # 其他偏导数为0 # 预测的观测值 z_pred np.array([x_pred[0], x_pred[1]]) # --- 2. 计算卡尔曼增益 --- S H_j P_pred H_j.T R_gps # 新息协方差 K P_pred H_j.T np.linalg.inv(S) # 卡尔曼增益 [5,2] # --- 3. 更新状态 --- y z - z_pred # 新息 x_est x_pred K y # --- 4. 更新协方差 (使用更稳定的约瑟夫形式) --- I np.eye(5) P_est (I - K H_j) P_pred (I - K H_j).T K R_gps K.T return x_est, P_est5.3 参数调校与初始化决定滤波器性能的关键Q矩阵过程噪声这是最难调的部分。它代表了模型的不确定性。位置和速度的过程噪声取决于你对运动模型匀速转向的信心。车辆机动性越强Q中对应位置和速度的方差应设得越大。可以从一个较小的值开始如diag([0.1, 0.1, 0.5, 0.5, 0.01])根据新息序列调整。偏航角的过程噪声主要取决于陀螺仪的零偏稳定性。如果角速度yaw_rate来自IMU且噪声大那么Q[4,4]偏航角噪声应该相应增大。R矩阵测量噪声相对容易确定。GPS噪声查看GPS模块的数据手册通常给出CEP或RMS值。例如如果GPS水平精度是3米RMS可以设R_gps diag([9, 9])方差标准差²。初始化x0如果有初始GPS信号可以用它初始化位置速度初始为0偏航角可以从电子罗盘或首次GPS航向获得。P0反映你对初始猜测的不确定性。例如位置初始不确定性可以设得大一些如100 m²速度不确定性也较大如25 (m/s)²偏航角不确定性如0.1 rad²。实操心得调试EKF时不要只看最终输出轨迹是否平滑。一定要绘制并分析新息序列。理想情况下新息应该是一个零均值的白噪声。如果新息出现明显的趋势或自相关说明你的Q或R设置不当或者模型有误。另外可以绘制协方差矩阵对角线元素各状态方差的变化它们应该随着滤波进行而收敛到一个稳定值。如果方差不断增长说明滤波器在“失去信心”很可能发散了。6. 超越基础EKF在复杂系统中的高级话题与实战陷阱当你把基础的EKF跑通后会遇到更实际、更棘手的问题。这些问题往往决定了你的滤波器能否在真实产品中稳定工作。6.1 异步多传感器融合与时间对齐现实中GPS、IMU、摄像头等传感器数据到达的频率和时刻各不相同。一个10Hz的GPS和一个100Hz的IMU如何融合核心策略以最高频率运行预测步骤通常以IMU或系统时钟的频率为基准在每个IMU数据到来时都执行一次预测步骤时间更新。这保证了状态估计能以最高的时间分辨率向前传播。异步更新当GPS或其他低频传感器数据到达时执行一次更新步骤测量更新。这里的关键是时间对齐。问题GPS数据包带有时间戳t_gps而你的滤波器当前状态估计对应的是最新时间t_now。t_gps很可能小于t_now。解决你需要维护一个状态历史缓冲区或者进行状态回溯。更常见的简化方法是在GPS数据到达时计算其时间戳与上一次预测时间之间的差值dt_gps然后用这个dt_gps和对应的IMU数据可能需要插值进行一次“从t_gps时刻到当前t_now时刻”的预测将状态和协方差“前向预测”到GPS数据对应的时刻然后在该时刻进行更新。更新后再立即用最新的IMU数据预测回当前时间t_now。这个过程称为“反向传播更新”或“延迟状态更新”在ROS的robot_localization等包中有成熟实现。6.2 观测模型非线性与奇点问题我们之前的GPS观测模型是线性的。但对于像雷达、激光雷达、摄像头这类传感器观测模型往往是非线性的。例子雷达观测雷达测量目标的距离r和方位角θ。状态是目标在直角坐标系下的位置(px, py)。观测函数为h(x) [sqrt(px^2 py^2), atan2(py, px)]^T这个函数的雅可比矩阵我们之前推导过。这里存在一个奇点问题当px和py都为0时目标在雷达正上方方位角θ的定义是不确定的其偏导数趋于无穷大导致H_j矩阵计算失败。应对方法工程规避在代码中增加判断当目标非常接近原点时采用一个退化的观测模型例如只使用距离信息或者给方位角一个固定的、很小的噪声。使用不同的参数化例如对于角度使用四元数或旋转矩阵来代替欧拉角可以避免万向节死锁和奇点但会引入额外的约束如四元数归一化需要在EKF中特殊处理误差状态卡尔曼滤波常用于此。6.3 一致性检查与故障检测让滤波器更健壮一个工业级的滤波器绝不能对错误的测量数据照单全收。新息检验计算新息的马氏距离d^2 y^T * S^(-1) * y其中y是新息向量S是新息协方差矩阵。理论上d^2应服从自由度为测量维度m的卡方分布。可以设置一个阈值例如对应95%置信区间如果d^2超过该阈值则认为当前测量是一个异常值可能来自传感器故障或多路径效应等应拒绝此次更新或使用极大的R矩阵进行更新即几乎忽略该测量。协方差矩阵有效性检查定期检查P矩阵是否保持对称正定。如果不是进行复位或采用平方根滤波算法如SR-EKF, UKF它们能更好地保证数值稳定性。传感器健康状态管理对于多传感器可以设计简单的投票机制或基于新息的一致性检验来动态调整各传感器的权重即调整R矩阵甚至剔除故障传感器。6.4 从EKF到误差状态卡尔曼滤波在涉及姿态估计尤其是3D旋转时直接对姿态如四元数、欧拉角进行EKF会遇到问题。因为姿态空间不是欧几里得空间存在约束如四元数必须归一化。直接加减操作可能破坏约束。误差状态卡尔曼滤波是一种更优雅的解决方案。其核心思想是状态向量分为两部分一个“名义状态”如四元数、位置、速度和一个“误差状态”小量的角度误差、位置误差、速度误差。名义状态使用完整的非线性模型如四元数积分进行传播不受线性化误差影响。误差状态是一个小量在其上定义EKF。因为误差很小线性化假设非常合理。ESKF的更新步骤只更新误差状态然后将误差状态修正到名义状态之后将误差状态重置为零。ESKF特别适用于IMU/GPS融合的导航定位是当今许多无人机、机器人定位算法的主流选择。它比直接对四元数做EKF更稳定、更准确。从标准KF到EKF再到UKF、ESKF是一个不断应对现实世界非线性、非高斯、复杂约束挑战的过程。理解KF/EKF的基础原理和局限是你踏入这个领域并最终能游刃有余地选择和应用更高级滤波器的基石。记住没有“最好”的滤波器只有“最适合”当前问题约束和资源的滤波器。