4. 测量方程：线性测量方程、非线性测量方程、测量噪声协方差矩阵
好，咱们接着聊。前面我们把状态方程讲透了，说白了就是系统自己怎么演化。但光有状态方程还不够——你得有传感器去“看”这个世界吧？这个“看”的过程，就是测量方程要干的事。
我个人习惯把测量方程比作“眼睛”。状态方程是大脑里的模型，测量方程就是眼睛看到的东西。两者一结合，才能做出靠谱的估计。
4.1 线性测量方程
先讲最简单的。线性测量方程长这样：
z = H * x + v其中：
- z 是测量值（传感器读数）
- x 是状态向量
- H 是测量矩阵（把状态映射到测量空间）
- v 是测量噪声
说白了，就是状态和测量之间是线性关系。比如你用尺子量长度，测量值 = 真实长度 + 误差。这就是线性。
实际例子：
假设你在跟踪一辆车的位置。状态是 [位置, 速度]。你用一个GPS接收机测位置。那么：
z = [1 0] * [位置; 速度] + vH矩阵就是 [1 0]，只提取位置分量。
我在项目中遇到过一个问题：有人把H矩阵写反了，结果滤波器发散得一塌糊涂。排查了两天才发现——嗯，矩阵维度对不上。所以写代码时一定要检查H的维度，别想当然。
4.2 非线性测量方程
现实世界哪有那么多线性？大多数传感器都是非线性的。比如：
- 雷达测距：距离 = sqrt(x² + y²)
- 角度测量：θ = arctan(y/x)
- 视觉SLAM中的重投影误差
非线性测量方程一般写成：
z = h(x) + v这里的 h(·) 是个非线性函数。你不能直接用矩阵乘法了。
注意：非线性测量方程不能直接套用标准卡尔曼滤波。你需要用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）。
EKF的做法是对h(x)做一阶泰勒展开，求雅可比矩阵。说白了就是“线性化”。
我曾经在一个无人机项目中用过EKF。测量方程是视觉特征点的像素坐标，那叫一个复杂。h(x)里包含了相机内参、外参、旋转矩阵...我第一次写的时候头都大了。后来学乖了，先用仿真验证雅可比矩阵的正确性，再上真机。
4.3 测量噪声协方差矩阵 R
这个R矩阵，我愿称之为“传感器的信任度”。
R矩阵的定义：
R = E[v * v^T]其中v是测量噪声向量。R矩阵的对角线元素是各传感器的方差，非对角线元素是不同传感器之间的协方差。
我的经验：R矩阵的取值直接影响滤波器的收敛速度和稳态精度。
- R设得太小：滤波器过于相信测量值，容易受噪声干扰
- R设得太大：滤波器反应迟钝，跟踪滞后
举个例子。假设你有两个传感器：
- GPS：位置测量，标准差5米 → 方差25
- IMU：加速度测量，标准差0.1 m/s² → 方差0.01
那么R矩阵就是：
R = [25 0
0 0.01]这里假设两个传感器独立，所以非对角线元素为0。
我曾经犯过一个错误：把GPS的方差设成了1米。结果滤波器疯狂抖动，因为实际GPS噪声远大于我的假设。后来老老实实做了半小时的静态数据采集，统计出真实方差，问题才解决。
4.4 知识体系总览
下面这张图，是我自己画的。它把测量方程的核心逻辑串起来了：
4.5 实际工程中的注意事项
最后，分享几个我在工程中踩过的坑：
- R矩阵要在线调整吗？ 我的建议是：先离线标定好。除非传感器特性随时间变化明显（比如温度漂移），否则别动它。
- 测量异常值怎么处理？ 加一个卡方检验。当测量残差过大时，直接丢弃该测量值。我吃过这个亏——有一次GPS跳了100米，滤波器直接崩了。
- 多传感器融合时R怎么设？ 按信噪比来。信噪比高的传感器，R设小点；信噪比低的，R设大点。别搞反了。
一句话总结：测量方程决定了你“看”得清不清，R矩阵决定了你“信”得有多深。两者配合好，滤波器才能干活。
好了，这一节就到这。测量方程这块内容不多，但很关键。你想想看，如果传感器模型建错了，后面再好的滤波器也白搭。所以，花时间把测量方程搞透，绝对值。