K̂

Kalman Filter

Advanced
运行在线示例 免费 · 无需注册 · 直接在浏览器中运行

KalmanFilter 模块实现用于单测量值二阶线性动态系统最优状态估计的离散时间卡尔曼滤波器。通过将物理模型先验预测与加权有噪传感器观测融为一体(权重取决于过程噪声协方差 QQ 与测量噪声协方差 RR),该模块能够准确估计难以直接测量的隐状态(如位置与速度),并实时追踪估计不确定度(σ=Pii\sigma = \sqrt{P_{ii}})。在属性编辑器中,所有矩阵与状态向量(a、b、c、q、xInit、pInit)均在直观的 2×22 \times 2 矩阵及双状态向量表格中编辑,归整为三组清晰的参数面板(_时序与模型类型_、_状态空间模型_ 以及 _噪声与初始协方差_)。

视频教程

数学模型

滤波器根据标量测量值 z∈Rz \in \mathbb{R} 与可选控制输入 u∈Ru \in \mathbb{R} 估计系统状态 x∈R2\mathbf{x} \in \mathbb{R}^2。

对于连续动态模型(x˙=Acx+Bcu+w\dot{\mathbf{x}} = \mathbf{A}_c \mathbf{x} + \mathbf{B}_c u + \mathbf{w},噪声谱密度为 Qc\mathbf{Q}_c),在采样周期 TsT_s 内进行严格解析离散化:

A=eAcTs,B=∫0TseAcτdτ Bc,Q=∫0TseAcτQceAcTτdτ\mathbf{A} = e^{\mathbf{A}_c T_s}, \quad \mathbf{B} = \int_0^{T_s} e^{\mathbf{A}_c \tau} d\tau \, \mathbf{B}_c, \quad \mathbf{Q} = \int_0^{T_s} e^{\mathbf{A}_c \tau} \mathbf{Q}_c e^{\mathbf{A}_c^T \tau} d\tau

在每个离散时间步 kk,滤波器按序执行两项阶段计算:

1. 时间更新(预测):

x^k−=Ax^k−1+Buk−1Pk−=APk−1AT+Q\begin{aligned} \hat{\mathbf{x}}_k^- &= \mathbf{A}\hat{\mathbf{x}}_{k-1} + \mathbf{B}u_{k-1} \\ \mathbf{P}_k^- &= \mathbf{A}\mathbf{P}_{k-1}\mathbf{A}^T + \mathbf{Q} \end{aligned}

2. 测量更新(校正):

预测测量值 z^k=Cx^k−+Duk+offset\hat{z}_k = \mathbf{C}\hat{\mathbf{x}}_k^- + D u_k + \text{offset},新息 yk=zk−z^ky_k = z_k - \hat{z}_k,以及新息方差 SkS_k:

Sk=CPk−CT+RS_k = \mathbf{C}\mathbf{P}_k^-\mathbf{C}^T + R

最优卡尔曼增益 Kk\mathbf{K}_k 与更新后的后验状态估计 x^k\hat{\mathbf{x}}_k 为:

Kk=Pk−CTSk−1,x^k=x^k−+Kkyk\mathbf{K}_k = \mathbf{P}_k^-\mathbf{C}^T S_k^{-1}, \quad \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k y_k

采用 Joseph 对称稳定化形式更新误差协方差矩阵:

Pk=(I−KkC)Pk−(I−KkC)T+KkRKkT\mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k\mathbf{C})\mathbf{P}_k^-(\mathbf{I} - \mathbf{K}_k\mathbf{C})^T + \mathbf{K}_k R \mathbf{K}_k^T

通过归一化新息平方监控滤波器统计一致性:NISk=yk2/Sk\text{NIS}_k = y_k^2 / S_k。

输入与输出

Direction ID Label 类型 Status
→ In in1 z number Required
→ In in2 u number Optional
→ In in3 Reset number Optional
→ In in4 Dropout number Optional
→ In in5 Offset number Optional
← Out x_hat_0 x̂₀ number Output
← Out x_hat_1 x̂₁ number Output
← Out sigma_0 σ₀ number Output
← Out sigma_1 σ₁ number Output
← Out k_gain_0 K₀ number Output
← Out k_gain_1 K₁ number Output
← Out z_hat ẑ number Output
← Out innovation Innov. number Output
← Out innovation_cov S number Output
← Out nis NIS number Output

参数

参数 Label 类型 默认值 描述
sampleTime Sample Time number 1 Filter period in seconds
model Model select continuous Continuous: dx/dt = A·x + B·u, discretised exactly for the sample time. Discrete: x[k+1] = A·x[k] + B·u[k], already for this sample time
a A (2x2) vector 0 1; 0 0 State matrix, row by row
b B (2x1) vector 0 1 Input matrix
c C (1x2) vector 1 0 Measurement row
d D number 0 Direct feedthrough from u to the measurement
offset Measurement Offset number 0 Constant term in z = C·x + D·u + offset, e.g. the intercept of a linearised curve
q Q vector 0 0.01 Process noise: two numbers for the diagonal, four for the full matrix. Per second (a density) for a continuous model, per sample for a discrete one
r R number 1 Measurement noise variance
xInit Initial Estimate vector 0 0
pInit Initial Covariance vector 1 1 Two numbers for the diagonal, four for the full matrix

使用示例

示例 1: 有噪雷达测距信号下的速度与位置跟踪

利用带高斯噪声的雷达测距数据(R=4.0 m2R = 4.0\text{ m}^2)估计车辆位置 xx 与速度 vv。配置连续矩阵 a = "0 1; 0 0"、b = "0; 1"、c = "1 0" 及过程噪声 q = "0.01 0.1"。滤波器准确重构平滑速度,伴随稳定的 2σ2\sigma 不确定度带,且将位置噪声降低了 70% 以上。

示例 2: 传感器丢帧容错的陀螺仪姿态估计

在遥测通信间歇性中断工况下估计角位移。角度传感器接入 z 端口,状态标志接入 Dropout 端口。当通信丢失(Dropout > 0.5)时,滤波器跳过测量更新,仅依靠系统模型先验预测;估计不确定度 σ\sigma 平滑放大,直至通信恢复。

说明与最佳实践

  • 直观 2×22 \times 2 矩阵与向量编辑网格: 无需记忆并手动输入分号分隔符,直接在交互式表格中修改 A,B,C,Q,x0,P0\mathbf{A}, \mathbf{B}, \mathbf{C}, \mathbf{Q}, \mathbf{x}_0, \mathbf{P}_0 的每个数值单元。
  • NIS 统计一致性调谐: 归一化新息平方(NIS=y2/S\text{NIS} = y^2 / S)的时间平均值在滤波器调优良好时应接近 1.0。若长期大于 1.0 说明过于自信(QQ 或 RR 过小);小于 1.0 则说明估计过于保守。
  • Joseph 对称稳定协方差算法: 测量校正采用 Joseph 公式,严格保证矩阵 P\mathbf{P} 的半正定性,规避截断误差引起的数值发散。
  • 仿射线性化工作点偏置: 参数与端口 offset 支持围绕非零工况点追踪,避免污染系统的动态转移矩阵 A\mathbf{A}。
  • 解耦计算架构: 离散卡尔曼滤波严格按固定周期 TsT_s 执行,允许被控物理连续对象由高阶变步长 ODE 求解器(RK4, TR-BDF2)精密积分。

常见工程问题解答 (FAQ)

Q: How to tune process noise covariance Q vs measurement noise covariance R?

RR is directly obtained from sensor characterization (variance of stationary sensor noise σ2\sigma^2). QQ is a design tuning parameter representing unmodeled dynamics and input disturbances. If Q≫RQ \gg R, the filter places high trust in sensor measurements (fast dynamic tracking, less noise filtering). If R≫QR \gg Q, the filter places high trust in the system model (smooth estimates, but slower response to unexpected physical maneuvers).

相关模块

  • StateSpace: 连续时间多输入多输出状态空间被控对象模块。
  • RandomNumber: 生成高斯白噪声以验证滤波器的降噪性能。
  • NumericDisplay: 实时 LCD 数值显示仪表。
  • uPlotDisplay: 多通道示波器,用于监测状态估计值与不确定度包络。