BlockWerk 文档
ENNLDEFRESZH
进入画布
K̂

Kalman Filter

Advanced

KalmanFilter 模块用于从含有噪声的观测信号中在线估计线性系统的内部隐藏状态。在每个采样步长内，该模块根据系统模型和已知控制输入先验预测状态，随后根据实时测量值进行后验校正，并通过过程噪声协方差 Q 与测量噪声方差 R 的权衡实现统计最优估计。

支持直接以连续时间状态方程（dx/dt = A·x + B·u，Q 为每秒功率谱密度）定义物理系统，算法利用矩阵指数和 Van Loan 方法进行高精度精确离散化。支持双状态、单输出系统，广泛适用于位置/速度联合跟踪、带未知零偏的一阶系统以及电池等效电路线性化状态估计。除状态估计值外，模块还输出标准差不确定度（σ）、卡尔曼增益以及归一化新息平方（NIS，用于检验滤波调谐是否一致）。该模块与求解器时钟精确对齐，传感器信号中断时可无缝降级为纯预测推算，且支持闭环反馈连接。

输入端口。 z 为测量值，u 为已知控制输入。Reset 触发时重置状态至初值，Valid 低于 0.5 时模块仅执行先验预测而跳过更新校正（未连接时默认有效）。

Offset 端口用于表示测量方程的常量项 z = C·x + offset。对于非线性曲线（如开路电压曲线 OCV-SOC），在工作点处的泰勒展开属于仿射形式；连接 Offset 端口可直接配合查表模块（LookupTable）实现工作点动态偏移补偿。

在 BlockWerk 中仿真

浏览器即开即用的交互式仿真环境 — 无需安装，由 WebAssembly 强力驱动。

在 BlockWerk 中仿真 →

数学模型

连续系统：x˙=Acx+Bcu+w,E[ww⊤]=Qc δ(t−s)A=eAcTs,B=∫0TseAcs ds Bc,Q=∫0TseAcsQc eAc⊤s ds预测步：x^−=Ax^+Buk−1,P−=APA⊤+Q更新步：S=CP−C⊤+R,K=P−C⊤/Sx^=x^−+K (z−z^),z^=Cx^−+Du+offset,P=(I−KC)P−(I−KC)⊤+KRK⊤σi=Pii,NIS=(z−z^)2/S\text{连续系统：}\quad \dot{x} = A_c x + B_c u + w,\quad E[w w^\top] = Q_c\,\delta(t-s) A = e^{A_c T_s},\quad B = \int_0^{T_s} e^{A_c s}\,ds\,B_c,\quad Q = \int_0^{T_s} e^{A_c s} Q_c\, e^{A_c^\top s}\,ds \text{预测步：}\quad \hat{x}^- = A\hat{x} + B u_{k-1},\qquad P^- = A P A^\top + Q \text{更新步：}\quad S = C P^- C^\top + R,\qquad K = P^- C^\top / S \hat{x} = \hat{x}^- + K\,(z - \hat{z}),\quad \hat{z} = C\hat{x}^- + D u + \text{offset},\quad P = (I - KC)P^-(I - KC)^\top + K R K^\top \sigma_i = \sqrt{P_{ii}},\qquad \text{NIS} = (z - \hat{z})^2 / S

其中：

  • x̂ 为二维状态估计向量，P 为协方差矩阵。
  • z 为测量输入，u 为已知输入（零阶保持）。
  • 连续时间模型通过矩阵指数法离散化，因此修改仿真步长不会破坏滤波参数物理一致性。
  • K 为卡尔曼增益向量，S 为标量新息方差。
  • 第一个步长直接校正初始估计而不预测；Valid < 0.5 时跳过更新校正。

输入与输出

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

使用示例

滤除正弦信号中的强高斯噪声

包含高斯随机噪声（σ = 0.15）的慢速正弦输入送入 50 Hz 恒速卡尔曼滤波器，滤波估计将均方根误差从 0.150 大幅压缩至 0.040。

无测速机工况下的速度重构

利用带量化阶梯噪声的位置传感器测量值，同时精确重建平滑位置与速度，避免了数值微分造成的噪声放大。

说明与最佳实践

  • σ 输出：在模型准确的前提下，x̂₀ ± 2σ₀ 构成 95% 置信区间。通过 NIS 端口监控一致性（理论时间均值应为 1）。
  • Valid 输入：用于连接传感器通信健康状态。低于 0.5 时停止校正，σ 随纯推算逐步放大直至恢复有效数据。
  • 闭环控制：估计状态可直接接入反馈控制器；由于预测步采用上一时刻控制量 u_{k-1}，当 d = 0 时不会产生代数环。
  • 鲁棒性设计：若参数矩阵维度异常，模块自动降级为默认安全模型，杜绝 NaN 扩散。

相关模块