3. 线性代数基础回顾

各位同学，咱们今天聊点硬核的。线性代数，说白了就是机器人状态估计的「通用语言」。你想想看，一个机器人要感知世界，它脑子里全是矩阵和向量在转。我当年刚入行时，觉得这玩意儿就是纯数学，直到第一次在卡尔曼滤波里栽了跟头……嗯，从那以后我老实了，基础不牢，地动山摇。

3.1 向量空间：机器人的「坐标系世界观」

什么是向量空间？别想得太玄乎。你就把它理解成一个「允许你做加法和数乘的集合」。机器人为什么需要这个？因为它的位置、速度、姿态，全都可以抽象成向量。

举个例子，一个机器人在二维平面上移动，它的位置就是 [x, y]^T。这个向量属于 R² 空间。如果它还能旋转，那姿态角 θ 就属于另一个空间。我习惯把向量空间想象成「机器人能到达的所有状态的集合」。

这里有个坑。我曾经在项目里用了一个不封闭的向量集来表示机器人的朝向，结果角度一过 360°，计算直接崩了。所以，一定要确认你的状态量构成一个合法的向量空间。

3.2 矩阵运算：状态估计的「加减乘除」

矩阵运算，说白了就是批处理。你有一堆传感器数据，一个一个算太慢，用矩阵一口气搞定。

咱们重点看几个在机器人里天天用的操作：

• 矩阵乘法：旋转矩阵 R 乘以一个向量 v，就是让这个向量转一下。我习惯把矩阵乘法理解为「线性变换的复合」。
• 矩阵求逆：在最小二乘里，(A^T A)^{-1} A^T b 是家常便饭。但注意，矩阵必须可逆，否则你的解不唯一。
• 转置：把行和列互换。在协方差矩阵里，转置用来保证对称性。

// 一个简单的 2D 旋转矩阵示例
// 绕原点旋转 θ 角度
R = | cosθ  -sinθ |
    | sinθ   cosθ |

// 应用：将点 (x, y) 旋转
new_point = R * [x, y]^T

3.3 特征值与特征向量：系统的「固有频率」

特征值和特征向量，听起来很抽象，但你可以这么理解：特征向量是系统「最稳定」的方向，特征值是那个方向上的「放大倍数」。

为什么会这样？因为对于矩阵 A，如果存在一个非零向量 v 使得 A v = λ v，那么 v 就是特征向量，λ 就是特征值。这意味着，在这个方向上，矩阵的作用只是拉伸或压缩，不会改变方向。

在机器人里，特征值分解常用于：

• 主成分分析（PCA）：找到数据方差最大的方向。我曾在激光雷达点云降维时用过，效果立竿见影。
• 系统稳定性分析：如果状态转移矩阵的特征值都在单位圆内，系统就是稳定的。否则……嗯，你的机器人可能会「抽风」。

3.4 协方差矩阵：不确定性的「地图」

协方差矩阵，是机器人状态估计里最重要的概念之一。它描述的是：你的估计有多「不确定」，以及各个状态量之间如何相互影响。

举个例子，你用一个 GPS 和一个 IMU 融合定位。GPS 的噪声是 σ_gps²，IMU 的噪声是 σ_imu²。如果它们独立，协方差矩阵就是对角阵：

P = | σ_gps²    0    |
    | 0      σ_imu²  |

但如果它们之间有相关性（比如 GPS 受磁场干扰，IMU 也受影响），那非对角线元素就不为零了。我习惯把协方差矩阵画成一个椭圆——长轴方向表示不确定性最大的方向，短轴方向表示不确定性最小的方向。

在实际项目中，协方差矩阵的更新是卡尔曼滤波的核心步骤。你想想看，每次观测后，不确定性应该减小，所以协方差矩阵的「迹」（对角线之和）会下降。如果它反而变大了，那你的传感器模型可能有问题。

3.5 知识体系总览

下面这张图，是我自己总结的线性代数在机器人状态估计中的逻辑关系。你看一遍，应该能有个整体印象。

3.6 实战中的避坑指南

最后，分享几个我踩过的坑，希望能帮你省点时间：

• 矩阵维度不匹配：写代码前，先在纸上画一下矩阵的维度。我见过太多人把 3x3 和 4x4 硬乘在一起。
• 协方差矩阵非正定：数值误差会导致协方差矩阵失去正定性。解决办法是定期做一次 P = (P + P^T) / 2 强制对称，再加一个小量 εI 保证正定。
• 特征值计算精度：对于大矩阵，直接用 eig() 可能不稳定。我习惯用 SVD 代替，鲁棒性更好。

好了，这一章就到这里。记住，数学是工具，不是目的。你的目标是让机器人跑起来，而不是证明一个定理。