2. 状态空间模型：线性动态系统的数学描述

好，咱们进入第二章。状态空间模型，这个名字听起来有点唬人，对吧？

其实说白了，它就是一套描述系统怎么随时间演变的数学框架。你想想看，我们做多传感器融合，核心问题是什么？就是系统当前在哪儿、未来会到哪儿，以及我们怎么用观测数据去修正这个认知。状态空间模型，就是回答这三个问题的标准语言。

2.1 为什么需要状态空间模型？

我记得刚入行那会儿，处理一个简单的GPS+IMU融合问题。我直接用原始数据做加权平均，结果一塌糊涂。为什么？因为IMU有漂移，GPS有延迟，这两个传感器的误差特性完全不同，简单加权根本处理不了。

后来我意识到，我们需要一个统一的数学框架，把系统的运动规律和传感器的观测规律分开建模。这就是状态空间模型的价值——它把问题拆成了两个部分：

• 状态方程：描述系统内部状态如何随时间变化
• 观测方程：描述传感器观测值与状态之间的关系

这两个方程，就是卡尔曼滤波的基石。没有它们，滤波无从谈起。

2.2 状态方程：系统是怎么动的？

先看状态方程。对于一个线性动态系统，它的标准形式是：

x(k) = A * x(k-1) + B * u(k-1) + w(k-1)

这里每个符号都有明确的物理含义：

• x(k)：k时刻的状态向量。比如位置、速度、姿态角
• A：状态转移矩阵。它描述了上一时刻的状态如何影响当前时刻
• B：控制输入矩阵。它描述了外部控制量（比如油门、转向）如何影响状态
• u(k-1)：控制输入向量
• w(k-1)：过程噪声。它代表了模型的不确定性

我个人习惯把A矩阵叫做「预测核心」。为什么？因为它决定了系统在没有外部输入和噪声时的演化规律。

2.3 观测方程：传感器看到了什么？

状态方程描述了「真实世界」的演化，但真实状态我们是不知道的。我们只能通过传感器去观测。观测方程就是连接真实状态和传感器读数的桥梁：

z(k) = H * x(k) + v(k)

这里：

• z(k)：k时刻的观测向量。比如GPS给出的位置读数
• H：观测矩阵。它把状态空间映射到观测空间
• v(k)：观测噪声。它代表了传感器的测量误差

嗯，这里要注意。H矩阵的设计直接决定了你的传感器能不能「看到」状态。比如你只有GPS测位置，那H就是[1, 0]，只能观测到位置，看不到速度。如果你有雷达测距，那H就得根据几何关系来设计。

2.4 噪声的数学描述

过程噪声w和观测噪声v，是卡尔曼滤波里最容易被忽视的部分。很多人随便给个协方差矩阵就完事了，结果滤波效果一塌糊涂。

这两个噪声的假设是：

• 零均值高斯白噪声
• 互不相关
• 各自协方差矩阵已知：Q = E[w w^T]，R = E[v v^T]

Q和R的取值，说白了就是你对模型和传感器的信任程度。Q越大，说明你越不相信模型预测；R越大，说明你越不相信传感器读数。

我记得有一次做无人机高度融合，气压计和加速度计融合。我一开始把R设得很小，结果气压计一有波动，滤波就跟着剧烈抖动。后来我把R调大了一些，滤波就平滑多了。这就是调参的艺术。

2.5 完整的线性动态系统模型

把状态方程和观测方程放在一起，就构成了完整的线性动态系统模型：

状态方程：x(k) = A * x(k-1) + B * u(k-1) + w(k-1)
观测方程：z(k) = H * x(k) + v(k)

其中：
w ~ N(0, Q)
v ~ N(0, R)
E[w v^T] = 0

这个模型，就是卡尔曼滤波的输入。你给它这个模型，它就能告诉你最优的状态估计。

2.6 一个完整的例子

咱们来看一个实际例子。假设你要做一维运动物体的位置和速度估计，传感器只有位置测量。

系统参数：

• 采样时间 dt = 0.1秒
• 过程噪声标准差 q = 0.1 m/s²
• 观测噪声标准差 r = 0.5 m

模型建立：

状态向量：x = [位置, 速度]^T

状态转移矩阵：A = [[1, 0.1], [0, 1]]

观测矩阵：H = [1, 0]  // 只能观测到位置

过程噪声协方差：Q = [[0.01, 0], [0, 0.01]]  // 简化处理

观测噪声协方差：R = [0.25]  // 0.5²

你看，这个模型很简单，但它已经能描述一个基本的运动估计问题了。卡尔曼滤波就是基于这个模型，不断地做预测和更新。

2.7 小结

状态空间模型，说白了就是两句话：系统怎么动，传感器怎么看。把这两句话用数学公式写出来，卡尔曼滤波就能工作了。

我个人觉得，这个模型是卡尔曼滤波里最重要的一步。模型建得好，滤波就成功了一半。模型建得差，后面再怎么调参也救不回来。

下一章，咱们就基于这个模型，推导卡尔曼滤波的五个核心公式。准备好了吗？