第二章：数学基础回顾——矩阵运算、坐标变换、卡尔曼滤波基础、最小二乘法
各位同学，欢迎来到第二章。
说实话，很多做嵌入式移植的工程师，一听到「数学基础」四个字就想翻页。我当年也一样，觉得反正代码能跑就行，数学差不多得了。直到有一次，我在移植一个弹道预测算法时，坐标变换算错了，导致目标跟踪直接飞到了屏幕外面……嗯，那次调试了整整两天。
所以这一章，咱们把几个核心的数学工具过一遍。不搞纯理论推导，只讲你写代码时真正用得上的东西。
2.1 矩阵运算：嵌入式里的「数组操作」
矩阵运算说白了，就是多维数组的加减乘除。在导弹跟踪里，状态向量、协方差矩阵、观测矩阵，全是矩阵。
2.1.1 矩阵乘法——最常用的操作
我个人习惯把矩阵乘法记成「行乘列」。举个例子：
// 两个 2x2 矩阵相乘
A = [[a11, a12],
[a21, a22]]
B = [[b11, b12],
[b21, b22]]
C = A * B
C[0][0] = a11*b11 + a12*b21
C[0][1] = a11*b12 + a12*b22
C[1][0] = a21*b11 + a22*b21
C[1][1] = a21*b12 + a22*b222.1.2 矩阵转置与求逆
转置很简单，行变列。求逆就麻烦点，尤其是实时系统里。我建议：能不求逆就不求逆。卡尔曼滤波里的求逆，可以用 Cholesky 分解或者 LU 分解来替代，速度能快 3-5 倍。
| 操作 | 时间复杂度 | 嵌入式建议 |
|---|---|---|
| 矩阵乘法 | O(n³) | 小矩阵（n≤4）直接展开写 |
| 矩阵求逆 | O(n³) | 尽量用分解法替代 |
| 矩阵转置 | O(n²) | 注意内存访问模式 |
2.2 坐标变换：从雷达坐标系到弹体坐标系
导弹跟踪里，坐标变换是家常便饭。雷达测得的是极坐标（距离、方位角、俯仰角），但卡尔曼滤波通常用直角坐标。你得来回转换。
2.2.1 极坐标转直角坐标
// 极坐标 (r, az, el) -> 直角坐标 (x, y, z)
x = r * cos(el) * cos(az)
y = r * cos(el) * sin(az)
z = r * sin(el)这里有个坑：角度单位。我见过不止一个项目，因为角度用了度而不是弧度，导致目标位置差了十万八千里。嵌入式里三角函数库通常用弧度，记得转换。
2.2.2 旋转矩阵
弹体坐标系和惯性坐标系之间，需要旋转矩阵来转换。绕 Z 轴旋转 θ 角的矩阵长这样：
Rz(θ) = [[cosθ, -sinθ, 0],
[sinθ, cosθ, 0],
[0, 0, 1]]2.3 卡尔曼滤波基础：预测+修正的循环
卡尔曼滤波，说白了就是两步走：先猜，再看，然后修正。我刚开始学的时候，被那一堆公式吓住了。后来发现，核心就五个公式。
2.3.1 五个核心公式
- 状态预测： x̂ₖ⁻ = A·x̂ₖ₋₁ + B·uₖ
- 协方差预测： Pₖ⁻ = A·Pₖ₋₁·Aᵀ + Q
- 卡尔曼增益： Kₖ = Pₖ⁻·Hᵀ·(H·Pₖ⁻·Hᵀ + R)⁻¹
- 状态更新： x̂ₖ = x̂ₖ⁻ + Kₖ·(zₖ - H·x̂ₖ⁻)
- 协方差更新： Pₖ = (I - Kₖ·H)·Pₖ⁻
你想想看，这其实就是一个「信任度」的调整过程。卡尔曼增益 K 大，说明我更相信观测；K 小，说明我更相信模型预测。
2.3.2 嵌入式实现要点
- 用 float 还是 double？—— 大多数 ARM Cortex-M 用 float 就够了，但要注意精度损失
- 矩阵维度？—— 导弹跟踪通常用 6 维状态（位置+速度），最多 9 维
- 更新频率？—— 一般 100Hz-1000Hz，取决于雷达刷新率
2.4 最小二乘法：拟合与初始化
最小二乘法在导弹跟踪里，最常用的场景是初始化卡尔曼滤波。雷达刚捕获目标时，你需要用前几帧的数据来估计初始状态。
2.4.1 线性最小二乘
假设你有 n 个观测点 (tᵢ, yᵢ)，想拟合一条直线 y = a + b·t。公式很简单：
// 正规方程解法
θ = (Xᵀ·X)⁻¹ · Xᵀ · y
// 其中 X 是设计矩阵，第一列全1，第二列是时间 t2.4.2 递推最小二乘（RLS）
如果你需要实时更新参数，就用 RLS。它和卡尔曼滤波其实是一回事，只是视角不同。
// RLS 核心更新公式
Kₖ = Pₖ₋₁·xₖ / (λ + xₖᵀ·Pₖ₋₁·xₖ)
θₖ = θₖ₋₁ + Kₖ·(yₖ - xₖᵀ·θₖ₋₁)
Pₖ = (Pₖ₋₁ - Kₖ·xₖᵀ·Pₖ₋₁) / λ这里的 λ 是遗忘因子，一般取 0.95-0.99。λ 越小，对旧数据遗忘得越快，适合机动目标。
2.5 本章小结
这一章的内容，说白了就是给后面的算法移植打地基。矩阵运算是工具，坐标变换是桥梁，卡尔曼滤波是核心，最小二乘法是辅助。你不需要成为数学家，但得知道每个公式在代码里长什么样。
下一章，咱们就开始讲具体的算法移植流程了。到时候你会发现，今天这些数学基础，每一行都会出现在你的代码里。