信号处理学习笔记5：卡尔曼滤波理论

卡尔曼滤波用直白的话来讲就是有多个不确定的结果经过分析、推理和计算获得相对准确的结果。它的核心特点是能够预测数据的未来趋势\({x}_{k}^{ }\)结合当前数据进行修正使预测更加准确可以处理动态变化的系统比如目标跟踪、导航定位等能够区分信号和噪声即使数据被干扰依然能估计出真实值目录卡尔曼核心思想逻辑1世界有两个参考值都不完美逻辑2动态分配信任权重逻辑3闭环迭代更新误差Q过程噪声模型噪声R测量噪声传感器噪声P协方差不确定度 / 误差家底K卡尔曼增益动态权重系数x状态估计值最终输出预测与修正的 “双轨” 闭环逻辑第一阶段预测 (Predict) —— “我觉得应该是多少”第二阶段修正 (Update) —— “现实告诉我确实是多少”五大公式卡尔曼核心思想核心思想预测 观测融合动态加权迭代收敛逻辑1世界有两个参考值都不完美预测值理论推演靠物理 / 数学模型推下一步长期稳、不跳变但会慢慢漂移测量值传感器采样实时新鲜、跟得上变化但自带毛刺、噪声、突发干扰。逻辑2动态分配信任权重卡尔曼增益 K 是灵魂不固定死权重不像均值 / 中值滤波一刀切传感器噪很大 → 少信测量多信预测 → 曲线平滑模型不准 / 物体在快速动 → 多信测量少信预测 → 响应灵敏。逻辑3闭环迭代更新误差每算完一次就更新「我这次估计有多不靠谱」下一次计算自动参考形成自收敛闭环越迭代越稳。Q 管模型值易变就加大要稳就减小R 管传感器噪大就加大精准就减小P 管误差家底K 管动态天平。Q过程噪声模型噪声物理意义你建立的数学模型准不准、真实值会不会偷偷特性Q↑加大认为真实值一直在变 → 更相信测量 → 响应快、贴近采样Q↓减小认为真实值基本不动 → 死守预测 → 超平滑、但滞后。适用场景静止测温 / 静态 ADC → Q 极小陀螺仪动态转角、快速运动 → Q 加大。R测量噪声传感器噪声物理意义你的传感器烂不烂、毛刺大不大、精度高不高特性R↑加大嫌弃传感器噪声大 → 压低测量权重 → 强力去毛刺、超稳R↓减小信任传感器很准 → 跟着采样走、去噪弱。适用场景廉价 ADC、噪多陀螺仪 → R 开大高精度校准传感器 → R 开小。P协方差不确定度 / 误差家底含义当前估计值「有多不靠谱」迭代逻辑预测阶段放大不确定度修正阶段收缩压误差作用自动给 K 提供依据决定权重怎么分。K卡尔曼增益动态权重系数核心P、Q、R 算出来的自动天平K 越大 → 听传感器的K 越小 → 听模型预测的。x状态估计值最终输出融合后最优真实值就是你滤波后拿来用的数据。预测与修正的 “双轨” 闭环逻辑第一阶段预测 (Predict)—— 我觉得应该是多少用上一时刻最优真值 物理数学模型推算当前理论预测值同时放大误差不确定度 P承认模型会有漂移、真实值会变这一步不看当前传感器数据只靠历史经验推演。第二阶段修正 (Update)—— 现实告诉我实际是多少拿当前传感器实测值和刚才的预测值做对比算出误差差值用 P、Q、R 自动算出卡尔曼增益 K动态信任权重用 K 把误差差值加权补偿给预测值得到最终最优输出 x同时收缩更新误差 P把本次精度封存留给下一轮迭代用。第一阶段预测 (Predict) —— “我觉得应该是多少”核心动作根据上一时刻的最优值利用物理模型如匀速运动、惯性定律推算出当前时刻的理论值。它在做什么逻辑xk−​A⋅xk−1​“我已经知道上一秒测得的真实值是 10.0根据我设定的模型比如假设物体静止或者匀速运动我推测这一秒的真实值应该还是接近 10.0。依赖模型它不看此时此刻的传感器读数只靠公式算。趋势稳定因为是继承上一刻的结果所以预测值通常很平滑很稳。误差累积如果只靠预测不管传感器时间久了模型的误差会越来越大也就是常说的漂移。2. 对应你的场景场景在读取 IMU 陀螺仪数据。预测根据上一时刻的角度加上上一时刻的角速度 × 时间推算出当前的角度。问题这种推算非常依赖积分精度稍微有点积分误差角度就会慢慢飘走越算越不准。第二阶段修正 (Update) —— “现实告诉我确实是多少”核心动作利用当前时刻的传感器实测值去修正刚刚算出来的预测值得出最终的最优估计。逻辑xk​xk−​K⋅(zk​−xk−​)“刚才我猜是 10.0但我看了一眼传感器它现在报数是 12.5。我不能完全不信传感器因为它有噪声也不能完全不信我的模型它有漂移。所以我要取个中间值最终值 预测值 权重 × (传感器值 - 预测值)。”依赖观测它必须看传感器的实测数据zk​。纠错机制这是专门用来打脸预测的。如果预测错了比如传感器突然跳变修正环节会强力拉回结果消除漂移。动态权重这个权重就是卡尔曼增益 (K)。传感器准就多听点传感器吵就少听点。2. 对应的场景场景同样是 IMU 读数据。修正我推算出角度应该是 30∘但加速度计告诉我水平方向是 29.8∘。卡尔曼会综合二者给出一个 29.95∘ 这种更精确的结果。五大公式状态预测x^k−​Ax^k−1​→ 产出预测值对应你理论推演协方差预测Pk−​APk−1​AQ→ 产出放大 P加 Q承认模型有波动增益计算KPk−​RPk−​​→ 产出K 天平Q/R/P 共同决定信谁状态修正x^k​x^k−​K(zk​−x^k−​)→ 产出最终 x预测 实测加权纠偏协方差更新Pk​(1−K)Pk−​→ 产出收敛 P闭环留到下一轮