卡尔曼滤波（Kalman Filter）详解

卡尔曼滤波是一种在存在测量噪声和过程噪声时对动态系统状态做最优估计的递推算法。它把“模型预测”和“传感器测量”按统计意义融合得到比单独用模型或单独用传感器更可靠的状态估计。1. 要解决什么问题典型场景你有一个动态系统位置、速度、角度、电流等随时间变化。你有不完美的模型简化、参数误差、未建模扰动。你有带噪声的测量ADC、编码器、IMU 等。目标在每个时刻 kk得到对真实状态 xkxk​ 的最优估计 x^kx^k​在线、递推、计算量可控。“最优”在线性高斯假设下指估计误差方差最小与最小均方误差意义一致。2. 状态空间模型线性离散状态方程过程模型xkFkxk−1Bkuk−1wk−1xk​Fk​xk−1​Bk​uk−1​wk−1​观测方程测量模型zkHkxkvkzk​Hk​xk​vk​xkxk​状态向量如位置、速度。uk−1uk−1​已知输入如控制力矩对应的加速度项。zkzk​测量向量。wk−1wk−1​过程噪声通常假设 w∼N(0,Qk)w∼N(0,Qk​)。vkvk​测量噪声通常假设 v∼N(0,Rk)v∼N(0,Rk​)。核心假设标准卡尔曼系统是线性的F,HF,H 为线性矩阵。噪声是零均值高斯且过程噪声与测量噪声互不相关常还假设与初始状态不相关。初始状态有高斯先验。在这些假设下后验仍是高斯卡尔曼给出该高斯的均值与协方差的闭式递推。3. 算法结构预测 更新两步递推记 x^k∣k−1x^k∣k−1​ 为用 k−1k−1 时刻及以前信息对 xkxk​ 的预测Pk∣k−1Pk∣k−1​ 为对应误差协方差。x^k∣kx^k∣k​、Pk∣kPk∣k​ 为纳入测量 zkzk​ 之后的后验估计。3.1 预测时间更新x^k∣k−1Fkx^k−1∣k−1Bkuk−1x^k∣k−1​Fk​x^k−1∣k−1​Bk​uk−1​Pk∣k−1FkPk−1∣k−1Fk⊤QkPk∣k−1​Fk​Pk−1∣k−1​Fk⊤​Qk​含义按动力学把状态和不确定性往前推QkQk​ 表示模型不准带来的协方差增大。3.2 更新测量更新新息innovation预测测量与真实测量的差ykzk−Hkx^k∣k−1yk​zk​−Hk​x^k∣k−1​新息协方差SkHkPk∣k−1Hk⊤RkSk​Hk​Pk∣k−1​Hk⊤​Rk​卡尔曼增益决定更信模型还是更信测量KkPk∣k−1Hk⊤Sk−1Kk​Pk∣k−1​Hk⊤​Sk−1​后验状态与协方差x^k∣kx^k∣k−1Kkykx^k∣k​x^k∣k−1​Kk​yk​Pk∣k(I−KkHk)Pk∣k−1Pk∣k​(I−Kk​Hk​)Pk∣k−1​数值实现里有时用 Joseph 形式等改写 Pk∣kPk∣k​以保证对称、正定、数值稳定。3.3 直观理解增益 KkKk​RkRk​ 大测量很不可靠→ SkSk​ 大 → KkKk​ 小 → 更信预测。Pk∣k−1Pk∣k−1​ 大模型预测很不确定→ KkKk​ 大 → 更信本次测量。因此卡尔曼滤波本质是按不确定性大小加权融合。4. 与贝叶斯推断的关系可把每一步看成预测用 Chapman–Kolmogorov 式地把上一时刻后验推到当前时刻先验。更新用贝叶斯公式把先验与似然 p(zk∣xk)p(zk​∣xk​) 结合成后验。在线性高斯下这一步有解析解就是上面的公式。非线性时一般没有闭式高斯后验于是出现 EKF、UKF、粒子滤波等。5. 连续时间与离散时间控制与嵌入式里几乎都用离散卡尔曼固定采样周期 TsTs​。若先有连续模型 x˙AxBux˙AxBu常通过零阶保持等得到 F≈IATsF≈IATs​小 TsTs​ 的一阶近似或矩阵指数 FeATsFeATs​再配离散过程噪声 QdQd​由连续 QcQc​ 与 TsTs​ 导出。6. 非线性系统EKF 与 UKF简述实际电机、机器人、导航多为非线性xkf(xk−1,uk−1)w,zkh(xk)vxk​f(xk−1​,uk−1​)w,zk​h(xk​)v扩展卡尔曼EKF在 x^x^ 处对 f,hf,h 一阶泰勒展开用局部线性化的 Fk∂f∂xFk​∂x∂f​、Hk∂h∂xHk​∂x∂h​ 套标准卡尔曼。实现简单但强非线性或大初始误差时可能不稳定或偏差大。无迹卡尔曼UKF用确定性采样点sigma points近似传播均值和协方差常比 EKF 在非线性下更稳计算比粒子滤波小。还有 ESKF误差状态卡尔曼在惯导里很常见在主积分外对小误差状态做线性卡尔曼利于保持姿态约束等。7. 与 Luenberger 观测器、 αβαβ 滤波的关系Luenberger 观测器结构类似增益常按极点配置不一定显式用 Q,RQ,R 的随机模型。一阶低通 / αβαβ可看成极简、固定增益的平滑卡尔曼是时变增益、且自然处理多传感器与多维耦合。8. 工程上要注意的点问题说明Q,RQ,R 调参相对比例决定融合倾向可结合物理噪声、试验迭代。可观测性若 H,FH,F 结构导致某些状态从测量中“看不见”估计会漂或病态。数值协方差对称化、平方根滤波、Joseph 形式等提高稳定性。异步/多率预测按控制周期有测量时再更新可扩展为信息滤波等形式。异常值标准卡尔曼假设高斯野值需门限、Huber、鲁棒滤波或切换模型。9. 在电机/运动控制中的典型用途结合你当前工程背景常见用法包括速度/位置估计编码器位置差分会放大噪声用位置速度状态模型卡尔曼或观测器得到更平滑的速度用于速度环。无传感器或低分辨率编码器融合电流模型与测量改善低速估计。传感器融合如编码器 IMU、或多种观测同一状态。10. 小结卡尔曼滤波 线性高斯假设下的最小方差递推状态估计。计算上每步主要是矩阵乘加和一次 SkSk​ 求逆维数等于测量维数通常很小。非线性用 EKF/UKF强非线性、非高斯用粒子滤波等。工程成败常在于 状态与噪声模型是否合理、可观测性 以及 Q,RQ,R 是否与真实误差匹配。