控制工程入门：状态观测器和线性卡尔曼滤波器

Contents — find the section you need

机器人很少直接读取所需的状态。编码器可能提供位置信息，但无法提供速度信息；惯性测量单元 (IMU) 报告的加速度和角速率存在偏差；摄像头定位信息可能延迟到达或丢失。状态观测器利用被控对象模型以及预测输出与测量输出之间的差异来估计隐藏状态。线性卡尔曼滤波器 (KF) 是标准的观测器，它包含误差协方差，并利用假定的过程噪声和测量噪声来判断预测值与测量值的可信度。

关键不在于“平均传感器”，而在于明确数据代表的物理时间、假定的误差模型以及传感器不一致时的处理方式。一个明显错误的估计器会误导 PID、LQR 和 MPC 等控制算法。关于 ROS 2 的时序和执行边界，请阅读 ROS 2 入门；关于更广泛的传感器集成，请阅读传感器融合入门。

实践结论

观测者根据模型预测和新息更新 $\hat{x}$ 。仅通过选择更智能的滤波器无法唯一地重构不可观测的状态。
卡尔曼滤波器 (KF) 携带状态误差协方差 $P$ ，并使用过程协方差 $Q$ 和测量协方差 $R$ 来计算卡尔曼增益 $K$ 。
$Q,R$ 是对模型和传感器误差的假设，并非任意的“精度旋钮”。应使用新息、协方差、时间戳和故障数据来验证这些假设。
如果估计值过时、协方差过大、新息异常或变换失败，则传递降级状态以控制并根据危险情况减少或停止运动。

状态空间观测器和可观测性

对于离散线性系统，

x_{k+1}=A_dx_k+B_du_k+w_k,\qquad z_k=Cx_k+v_k

$x$ 代表真实状态， $u$ 代表已知输入， $z$ 代表测量值， $w$ 代表过程扰动， $v$ 代表测量噪声。Luenberger 观测器的形式如下：

\hat{x}_{k+1}=A_d\hat{x}_k+B_du_k+L(z_k-C\hat{x}_k)

最后一项是新息。高增益观测器 $L$ 能快速跟踪测量值，但会传递更多噪声；低增益观测器 $L$ 则能更长时间地信任模型，但对扰动的修正速度较慢。

图示：Duskcoil，概念图而非实测图。估计值和协方差是内部模型量，而非直接传感器观测值。

在理想的线性情况下，可观测性矩阵 $\mathcal O=[C^T,(CA)^T,\ldots]^T$ 必须满秩才能重建每个状态。仅位置信息双积分器的测量可以使速度随时间变化可观测，但采样率、量化、延迟和丢包决定了最终估计值的有效性。“可观测”并不能替代最大延迟和精度要求。

卡尔曼预测和更新

假设 $w\sim\mathcal N(0,Q)$ 和 $v\sim\mathcal N(0,R)$ 为零均值白高斯分布，则先验分布为：

\hat{x}_k^-=A_d\hat{x}_{k-1}^++B_du_{k-1},\qquad P_k^-=A_dP_{k-1}^+A_d^T+Q

$P$ 表示估计误差的不确定性，而不仅仅是状态的方差。在测量中断期间，模型传播通常会导致 $P^-$ 通过 $Q$ 增长。

对于新到达的测量值，计算新息 $\nu_k=z_k-C\hat{x}_k^-$ 及其协方差

S_k=CP_k^-C^T+R

然后使用以下参数进行更新：

K_k=P_k^-C^TS_k^{-1},\qquad \hat{x}_k^+=\hat{x}_k^-+K_k\nu_k,

P_k^+=(I-K_kC)P_k^-

有限精度可能会破坏 $P$ 的对称性或半正定性。通常优选使用Joseph形式， $P^+=(I-KC)P^-(I-KC)^T+KRK^T$ ；求解分解线性系统通常比显式求逆更安全。协方差矩阵 $P$ 应在每次更新后进行检查。

选择Q和R

$R$ 应反映实际振动、温度、光照、距离、数据包重试和量化条件下的测量误差，而不仅仅是目录精度值。 $Q$ 表示模型在一个周期内可能出现的误差：未知加速度、偏差随机游走、有效载荷变化或未建模的动态特性。过小的 $Q$ 表示对模型过于信任；过大的 $Q$ 表示测量噪声较大。

数值示例：根据位置测量估计速度

对于一维恒速状态 $x=[p\ v]^T$ ，采样周期为 $T$ ，

A_d=\begin{bmatrix}1&T\\0&1\end{bmatrix},\quad B_d=\begin{bmatrix}T^2/2\\T\end{bmatrix},\quad C=\begin{bmatrix}1&0\end{bmatrix}

仅测量位置。作为具体的无量纲更新，令 $T=1$ 、 $\hat{x}^-=[0\ 0]^T$ 、 $P^-=[[2,1],[1,1]]$ 、 $z=1$ 、 $R=1$ 和 $C=[1\ 0]$ 。则 $S=3$ 、 $K=[2/3\ 1/3]^T$ 和 $\hat{x}^+=[2/3\ 1/3]^T$ ；后验协方差为 $P^+=(I-KC)P^-=[[2/3,1/3],[1/3,2/3]]$ 。这仅展示了一次更新，并非对传感器精度的声明。较大的初始值 $P_{vv}$ 表示速度的不确定性。重复的位置创新推断出估计的速度 $\hat v$ ；数据丢失会导致预测持续运行，但会增加其不确定性。需要速度信息的控制器必须检查 $P_{vv}$ 、测量时长和残差状态，并在它们超出设定范围时减速或停止。

添加IMU有助于跟踪短期加速度，但忽略状态偏差会导致积分误差累积。当相机或激光雷达观测与已知地图、标记或其他参考点关联时，可以约束绝对位置；仅靠相对跟踪无法保证全局位置。GNSS还受卫星可见性、多径效应、更新速率、延迟和帧对齐的影响。对于非线性姿态或距离观测，可以使用EKF或UKF，但线性KF仍然存在 $Q,R$ 、测量协方差 $R$ 、时间、异常值和可观测性等问题。

归一化创新平方 $\nu^TS^{-1}\nu$ 有助于检测与其预测分布不一致的测量值。较大的值可能意味着异常值、错误的帧。时间戳错误、传感器故障或模型不匹配。对数据进行门控并非为了掩盖错误：它应该用于诊断并提供一种安全的降级模式。

代表性的 IMU/INS

代表性的深度相机

图片：Xsens MTi-G (Kallap85, CC BY-SA 4.0) 和 Intel RealSense D435（Marc Auledas，CC BY-SA 4.0），Wikimedia Commons。这些是代表性传感器，并非测量结果或推荐的组合系统。

PID、LQR 和 MPC 的提供

PID 可以使用原始滤波反馈，但其微分项和内部速度回路通常受益于估计值。LQR 假设多状态 $\hat{x}$ ；在分离原理条件下，LQR 增益和卡尔曼估计器构成 LQG 架构。饱和、延迟和非线性仍然会破坏实际应用中的鲁棒性。MPC 也依赖于及时的初始状态和扰动估计；基于过时的估计或过小的协方差进行优化，会为错误的实际情况创建一个看似完美的方案。

目标	典型控制	所需的估计器输出	故障响应
单速度/温度回路	PID	滤波测量/速度估计	传感器失效时停止积分
局部多状态稳定	LQR/LQG	同步 $\hat{x}$ 和有效性	不要过度依赖不断增长的协方差
约束路径/分配	MPC	状态、延迟、扰动、置信度	状态过时/求解器故障时减速或停止

机器人时序和故障安全操作

在 ROS 2 中，区分测量时间和消息到达时间。如果将延迟的相机数据更新为当前数据，则机器人会向过去状态偏移。选择并记录一种策略：回溯/重传历史记录、拒绝延迟数据，或将低级里程计与较慢的地图校正分开。ROS 2 入门和传感器融合入门中描述的 ROS 2 传输和坐标变换是滤波器方程之外的前提条件。

监控超时时间，协方差上限、重复的创新门故障、发散偏差、不可用的变换以及超时。如果所有测量值都消失，则不要仅依靠预测无限期运行。根据应用场景选择限速、停止、冗余或操作员交接。估计器重置可能会导致状态跳变，因此控制器参考值、积分状态和输出需要无扰处理。

实现清单

定义状态、测量值、输入、帧、测量时间和接收时间。
量化每个状态的可观测性、所需精度和最大延迟。
记录来自规范、数据和测试的 $Q,R,P_0$ 的证据。
监控 $P$ 、创新、门和超时的对称性/确定性。
注入 dropout、异常值、重定位和偏差变化的情况。
定义当估计质量下降时 PID/LQR/MPC 接收什么，并验证独立的安全措施。

参考文献

检查你的理解

当测量值更值得信赖时会发生什么？

在预测不确定性相同的情况下，较低的测量协方差会使更新值更倾向于测量值。不切实际的置信度会导致估计值跟随噪声变化。

Try it in the browser

Lab Change the conditions in a browser experiment在 Python 中运行卡尔曼滤波器：噪声、缺失数据和异常值