Observer 与 Kalman Filter

Predictx⁻=Ax+Bu, P⁻=APAᵀ+Q

→

Compareinnovation y−Cx⁻

→

WeightK=P⁻CᵀS⁻¹

→

Correctx⁺, P⁺

1. 确定性 Luenberger observer

对 xₖ₊₁=Axₖ+Buₖ, yₖ=Cxₖ，可写：

innovation/residual = y−ŷ 是测量与预测的差。estimation error 按 A−LC 演化；若系统 detectable，可选 L 让误差收敛。gain 太快会追 noise，太慢会保留 model bias。

2. 加入 stochastic noise

Q 表示 dynamics/process uncertainty，R 表示 sensor noise covariance。它们不是 LQR 的 Q/R；名字相同但含义、单位完全不同。

3. Kalman filter 两阶段

Predict:
  x̂⁻ = A x̂⁺ + B u
  P⁻ = A P⁺ Aᵀ + Q
Update:
  ν = y - C x̂⁻
  S = C P⁻ Cᵀ + R
  K = P⁻ Cᵀ S⁻¹
  x̂⁺ = x̂⁻ + Kν
  P⁺ = (I-KC)P⁻

P 是 estimation error covariance，K 是 Kalman gain。P⁻ 大或 R 小，就更信 measurement；sensor 很 noisy 时则更信 prediction。

4. 为什么 Q/R 不是随便调的旋钮

• Q 太小：过度自信模型，innovation 有结构且 correction 慢；
• Q 太大：estimate 跟 noise 走，state 抖；
• R 太小：过度相信 sensor，outlier 冲击大；
• R 太大：sensor 基本不起作用，bias 漂移。

最好从静态/动态 log 估 covariance，再用 normalized innovation consistency 检查，而不只是看 trajectory “顺不顺”。

5. EKF、UKF 与 nonlinear estimator

EKF 在当前 estimate 处 linearize f,h 并传播 Jacobian；UKF 用 sigma points 近似 nonlinear distribution。两者都把 posterior 近似为 Gaussian，遇到多峰 contact ambiguity、强非线性或错误 data association 会失效。factor graph/smoothing 可利用一段时间窗重估 past states，但带来 latency 与计算。

6. 机器人常估哪些东西

• floating-base pose/velocity 与 IMU bias；
• joint velocity、motor/link side state 与 elastic deflection；
• foot contact state、ground normal、contact force；
• camera–IMU extrinsics、clock offset；
• payload、friction 等慢变参数。

contact-aided estimation 把“接触足相对世界不动”当 pseudo-measurement；一旦 foot slip，这个 assumption 会把错误注入 base estimate。

7. 时间同步比公式更先

sensor timestamp、actuator command 生效时间和 estimator clock 必须统一。10 ms 的未补偿延迟在高速运动中会表现成固定 innovation 和假 velocity bias。实践需 buffer、interpolation、out-of-sequence handling，并记录采样时间而非只记录接收时间。

8. Estimator 健康指标

指标	异常含义
innovation mean	持续非零通常是 model/sensor bias
innovation covariance	与预测 S 不符说明 Q/R 或模型错
Mahalanobis distance / rejection rate	outlier、association 或 covariance 过小
P 的 eigenvalues	不确定性是否发散或虚假过度自信
latency / dropped messages	时序健康，而非估计公式本身

9. 标量 Kalman filter 走一遍

自测

1. innovation 是什么？

当前 measurement 与基于 prior estimate 的预测 measurement 之差，是 update 的新信息。

2. 增大 process Q 有什么直觉效果？

prediction covariance 增大，filter 更少相信模型、更快接受新 measurement。

3. foot-contact pseudo-measurement 何时危险？

足底实际滑动、接触误判或地面变形时，“足固定”假设错误。