Recursive estimation

卡尔曼滤波 Interactive Kalman filter simulation.

测量噪声标准差 σ 是真实世界加入的噪声；滤波器使用的 R 是它假设的测量方差。两者不一致时，曲线会出现过度平滑或过度抖动。

State: position / velocity dt = 1/30 s Joseph covariance update Repeatable noise sequence

实时估计 Real-Time Estimate

测量 RMSE · 最近最多 10 s

滤波 RMSE · 最近最多 10 s

位置增益 K₁

真实信号带噪测量 Kalman 估计模型内部 ±1σ

数学模型与校正说明 Mathematical Model and Notes

这里采用一维位置测量、二维状态的恒速模型，协方差更新使用 Joseph 形式提高数值稳定性。

适用假设

当初始状态与噪声服从高斯分布、系统线性、噪声为相互独立的白噪声且协方差已知时，卡尔曼滤波给出最小均方误差估计。Apollo 导航是它的早期著名应用之一，但并非适合用“最早应用于”来概括。

核心直觉：预测，然后用测量修正

卡尔曼滤波每一步都在回答同一个问题：这一次我应该更相信模型，还是更相信传感器？答案由卡尔曼增益 K 给出。

预测

用运动模型把上一时刻状态推到当前时刻。因为模型不完美，预测后的不确定度会增加。

创新

z-Hx⁻ 是测量与预测之间的差，叫 innovation。单次偏大可能只是随机噪声；持续偏置或归一化创新统计异常，才更能说明模型或噪声假设有问题。

更新

用 K 把创新加回预测状态。K 大则更信测量，K 小则更信模型。

标量入门版

如果只估计一个缓慢变化的量，例如温度，状态方差记为 p，过程噪声方差为 Q，测量噪声方差为 R：

\hat{x}^-=\hat{x},\qquad p^-=p+Q

K=\frac{p^-}{p^-+R}

\hat{x}=\hat{x}^-+K(z-\hat{x}^-)

p=(1-K)p^-

当 p^- >> R，说明模型很不确定、测量相对可靠，K 接近 1；反之 K 接近 0。

状态空间模型

\mathbf{x}_k=F\mathbf{x}_{k-1}+\mathbf{w}_k,\qquad z_k=H\mathbf{x}_k+v_k

F=\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix},\quad H=\begin{bmatrix}1&0\end{bmatrix},\quad \mathbf{x}=\begin{bmatrix}x\\v\end{bmatrix}

本页仿真虽然只画一条位置曲线，但滤波器内部同时估计位置和速度。只测位置仍可估计速度，是因为该系统在连续位置观测下可观；这依赖采样与模型，不能推广成“任何只测位置的系统都能估速度”。初值固定为 x̂₀=[0,0]ᵀ、P₀=I₂。

预测与更新

\hat{\mathbf{x}}_k^- = F\hat{\mathbf{x}}_{k-1},\qquad P_k^-=FP_{k-1}F^T+Q_d

Q_d=q_a\begin{bmatrix}\Delta t^3/3&\Delta t^2/2\\\Delta t^2/2&\Delta t\end{bmatrix}

K_k=P_k^-H^T(HP_k^-H^T+R)^{-1}

\hat{\mathbf{x}}_k=\hat{\mathbf{x}}_k^-+K_k(z_k-H\hat{\mathbf{x}}_k^-)

界面中的 qₐ 是滤波器假设的连续白加速度噪声功率谱密度，代码按采样周期 Δt 构造离散协方差矩阵 Qd，并不是把一个标量直接加到所有状态方差上。演示的真实轨迹是确定性的正弦、阶跃、斜坡或常值函数，没有随机注入过程噪声；这里的 qₐ 用来表示滤波器对恒速模型失配和未知加速度的容许程度。

Joseph 协方差更新

P_k=(I-K_kH)P_k^-(I-K_kH)^T+K_kRK_k^T

页面采用 Joseph 形式更新协方差；它与简化式在精确算术下等价，在浮点计算中更有利于保持矩阵的对称性与半正定性。

Q 与 R 的尺度

R 是测量噪声方差，单位是测量单位的平方；真实噪声标准差为 σ 时，真实测量方差是 σ²。Qd 是状态过程噪声协方差矩阵，各元素单位随状态而不同，因此不能把 Q/R 当作无量纲比值脱离模型比较。

图中的 ±1σ 来自滤波器内部的 √P₀₀，只表示在当前模型与协方差假设下的估计不确定度；当模型失配或 Q/R 设置错误时，它不是对真实误差覆盖率的保证。

这四类演示信号为什么故意“不完美”

正弦、周期阶跃和周期斜坡并不严格服从恒速加白加速度噪声模型。它们用于压力测试模型失配：平滑参数会降低噪声，却可能在转折与跳变处产生滞后。四条真实轨迹按下式生成，采样时刻固定为 tₖ=k/30 s：

p_{sin}(t)=1.9\sin(2\pi\cdot0.33t),\qquad p_{const}(t)=1.1

p_{step}(t)=\begin{cases}-1.4,&0\le(t\bmod8)<4\\+1.4,&4\le(t\bmod8)<8\end{cases}

p_{ramp}(t)=(0.55t\bmod5.6)-2.8

“周期阶跃”是每 4 s 在 ±1.4 之间切换的方波，不是只发生一次的阶跃。因此 RMSE 是这段特定演示数据上的比较指标，不是对其他轨迹的性能承诺。

RMSE 的定义与窗口

令 pₖ 为演示轨迹的真实位置、zₖ 为带噪测量、p̂ₖ 为滤波后的位置估计，则页面显示：

\mathrm{RMSE}_{z}=\sqrt{\frac{1}{N}\sum_{k=1}^{N}(z_k-p_k)^2}

\mathrm{RMSE}_{\hat p}=\sqrt{\frac{1}{N}\sum_{k=1}^{N}(\hat p_k-p_k)^2}

采样周期为 Δt=1/30 s，统计窗口取最近最多 N=300 个样本，因此最长为 10 s；刚启动时使用当前已有的全部样本。

实际调参顺序

先测 R：让传感器在静止或已知真值条件下采样，计算样本方差。
再调 Q：从偏小开始，逐步增大，直到估计能跟上真实变化但不过度跟随噪声。
实际应用应根据初始状态可信度设置 P0；本页为了固定对比，始终使用 P₀=I₂。
观察 innovation：如果残差长期偏一侧，可能是模型偏置；如果残差方差远大于预期，可能是 Q/R 设错。

本页固定随机噪声序列，便于公平比较参数。对当前四类演示信号，qₐ=2.00 是兼顾跟随与平滑的默认起点；qₐ=0.10 更适合观察“过度相信恒速模型”造成的滞后。

常见问题与扩展

估计滞后：Q 太小或 R 太大，滤波器过度相信模型。
输出抖动：Q 太大或 R 太小，测量噪声进入估计。
滤波发散：模型结构错、噪声不是白噪声、非线性太强，或数值协方差失去正定性。
非线性系统：可用 EKF 做一阶线性化；强非线性或姿态类问题常考虑 UKF 或粒子滤波。