矩阵乘法的Q15定点实现，让卡尔曼滤波不再拖后腿

前五篇文章里，我用手写定点函数替换了sinf()、atan2f()、sqrtf()、expf()、logf()。但真正让嵌入式工程师头疼的，不是单个数学函数，而是矩阵运算——卡尔曼滤波、姿态解算、神经网络推理，核心都是矩阵乘法。今天我们来手撕矩阵乘法。

01 为什么是矩阵乘法？

在嵌入式开发中，矩阵乘法的出场频率极高：

– 卡尔曼滤波：预测和更新步骤全是矩阵乘法

– 姿态解算：四元数更新、旋转矩阵计算

– 神经网络推理：全连接层就是矩阵乘法

– 最小二乘拟合：法方程求解

– 电机FOC：Clarke/Park变换本质是2×2矩阵乘法

浮点矩阵乘法在 Cortex-M4 上虽然可以用 FPU 加速，但矩阵维度越大，运算耗时越明显（运算量按 O(N³) 增长），且执行时间受数据影响（异常值、非规格化数会导致流水线停顿）。而定点 mat_mul_q15() 用纯整数运算，执行时间更固定，在无 FPU 的 M0/M3 上速度优势明显。需要注意的是，定点运算的累积误差通常比浮点大。浮点的相对误差固定在 1e-7 量级，而定点的绝对误差会随累加次数线性增长。因此，定点矩阵乘法适合对速度、确定性要求高，但对绝对精度要求不苛刻的场景。

02 矩阵乘法的特殊难点

和单个数学函数不同，矩阵乘法有三个额外的难点：

难点一：Q格式乘法会“放大”位宽

两个 Q15 数相乘，结果是 Q30：

Q15 × Q15 → Q30

必须右移 15 位才能回到 Q15。如果忘了移位，结果会大 32768 倍。

难点二：累加会溢出

矩阵乘法的每个输出元素是 N 个乘积之和。如果 N=8，每个乘积最大 32767×32767 ≈ 10⁹，累加 8 次就是 8×10⁹，远超 int32_t 的 2.1×10⁹。

必须用int64_t 累加，或者用 Q15 输入但中间累加用 Q30 格式。

难点三：数据布局影响Cache命中率

矩阵乘法是内存访问密集型操作。行优先和列优先的访问模式，在 Cortex-M7 的 Cache 下性能差异可达数倍。

03 核心思路：Q15输入，Q30累加，Q15输出

定点矩阵乘法的核心公式：

C[i][j] = Σ A[i][k] × B[k][j]

每一步：

1. A 和 B 的元素是 Q15

2. 乘积是 Q30

3. 累加用 int64_t（防止溢出）

4. 最后右移 15 位，回到 Q15

关键洞察：矩阵乘法的定点化不是“把浮点换成定点”那么简单，而是在每一步都精确控制Q格式的缩放。

04 定点矩阵乘法实现

基础版本（通用，任意维度）

#include <stdint.h>

/**

 * @brief  Q15 定点矩阵乘法

 * @param  A  M×K 矩阵，Q15 格式

 * @param  B  K×N 矩阵，Q15 格式

 * @param  C  M×N 输出矩阵，Q15 格式

 * @param  M  行数

 * @param  K  内维度

 * @param  N  列数

 */

void mat_mul_q15(const int16_t *A, const int16_t *B, int16_t *C,

                 int M, int K, int N) {

    for (int i = 0; i < M; i++) {

        for (int j = 0; j < N; j++) {

            int64_t sum = 0;

            for (int k = 0; k < K; k++) {

                sum += (int32_t)A[i * K + k] * B[k * N + j];

            }

            // 累加结果是 Q30，右移 15 位回到 Q15

            // 加 16384 实现四舍五入

            C[i * N + j] = (int16_t)((sum + 16384) >> 15);

        }

    }

}

优化版本一：循环顺序优化

上面的代码在k循环中访问 B[k*N+j]，这是列访问，在行优先存储中不连续。改成 `i-k-j` 循环顺序可以改善：

void mat_mul_q15_ikj(const int16_t *A, const int16_t *B, int16_t *C,

                     int M, int K, int N) {

    // 先清零

    for (int i = 0; i < M * N; i++) C[i] = 0;

    for (int i = 0; i < M; i++) {

        for (int k = 0; k < K; k++) {

            int16_t a = A[i * K + k];

            for (int j = 0; j < N; j++) {

                // C[i][j] += A[i][k] * B[k][j]

                // 这里用 int32_t 累加 Q30，注意溢出

                int32_t prod = (int32_t)a * B[k * N + j];

                // 需要把 prod 从 Q30 转成 Q15 再累加

                // 但这样会损失精度，更好的做法是用 int64_t

                C[i * N + j] += (int16_t)(prod >> 15);

            }

        }

    }

}

注意：这个版本为了减少内存访问，牺牲了精度（每次乘积先转Q15再累加）。推荐用基础版本，精度更高。

优化版本二：2×2 矩阵专用

对于 FOC 的 Clarke/Park 变换，2×2 矩阵乘法可以完全展开：

// [c00 c01]   [a00 a01]   [b00 b01]

// [c10 c11] = [a10 a11] × [b10 b11]

void mat_mul_2x2_q15(const int16_t *A, const int16_t *B, int16_t *C) {

    int64_t s00 = (int64_t)A[0]*B[0] + (int64_t)A[1]*B[2];

    int64_t s01 = (int64_t)A[0]*B[1] + (int64_t)A[1]*B[3];

    int64_t s10 = (int64_t)A[2]*B[0] + (int64_t)A[3]*B[2];

    int64_t s11 = (int64_t)A[2]*B[1] + (int64_t)A[3]*B[3];

    C[0] = (int16_t)((s00 + 16384) >> 15);

    C[1] = (int16_t)((s01 + 16384) >> 15);

    C[2] = (int16_t)((s10 + 16384) >> 15);

    C[3] = (int16_t)((s11 + 16384) >> 15);

}

展开后没有循环，编译器可以做更好的寄存器分配，速度比通用版本快 2~3 倍。

优化版本三：3×3 矩阵专用（用于卡尔曼滤波）

void mat_mul_3x3_q15(const int16_t *A, const int16_t *B, int16_t *C) {

    for (int i = 0; i < 3; i++) {

        for (int j = 0; j < 3; j++) {

            int64_t sum = 0;

            for (int k = 0; k < 3; k++) {

                sum += (int32_t)A[i*3+k] * B[k*3+j];

            }

            C[i*3+j] = (int16_t)((sum + 16384) >> 15);

        }

    }

}

3×3 矩阵是卡尔曼滤波的典型维度（状态向量3维），手写展开可以进一步优化。

05 精度分析

定点矩阵乘法的误差来源：

对于卡尔曼滤波、姿态解算、神经网络推理，0.01% 的相对误差完全够用。

06 性能实测

测试平台：GD32F303VGT6 @120MHz，DWT周期计数。

测试条件

– 矩阵维度：4×4，单次调用

说明：mat_mul_q15() 完全采用整数运算，因此开启或关闭 FPU 对其耗时没有影响。

07 应用场景

 卡尔曼滤波

// 预测步骤：x = F·x

mat_mul_q15(F, x, x_pred, 4, 4, 1);

// 协方差预测：P = F·P·Fᵀ + Q

mat_mul_q15(F, P, FP, 4, 4, 4);

mat_mul_q15(FP, F_T, P_pred, 4, 4, 4);

姿态解算

// 四元数更新：q = q + 0.5·q⊗ω·dt

// 四元数乘法是4×4矩阵乘法

mat_mul_q15(q_matrix, omega, q_dot, 4, 4, 1);

神经网络推理

// 全连接层：y = W·x + b

mat_mul_q15(W, x, y, 1, N, 1);

08 源码与工程

完整的Keil工程已上传Gitee，包含：

– GD32F303VGT6的完整工程

– `mat_mul_q15` 完整源码（通用版 + 2×2 + 3×3 + 4×4）

– 性能测试代码（使用DWT周期计数）

– 精度测试代码

Gitee地址：https://gitee.com/jervis_luo/gd32_mat_mul.git

09 总结

三点核心收获：

1. 定点矩阵乘法的核心是Q格式缩放——Q15×Q15→Q30，累加后右移15位回到Q15。

2. 累加必须用int64_t——防止N个乘积之和溢出int32_t。

3. 执行时间固定——没有循环和分支预测失败，对实时控制非常友好。

系列文章

1. fast_sin：比官方sinf快14倍

2. fast_atan2：FOC角度计算优化：128点查表法让atan2f快8倍

3. fast_sqrt：FOC矢量幅值计算优化：256点查表+牛顿迭代，让sqrtf快2.5倍

4.fast_exp：拆整数+查表+插值，关闭FPU快9倍

5.fast_log：提取二进制指数+查表，让对数运算快11倍

6.mat_mul_q15：定点矩阵乘法，让卡尔曼滤波不再拖后腿(本文)

后记：写完这篇文章后，我把 mat_mul_q15() 用在了自己做的卡尔曼滤波姿态解算里。原本用浮点矩阵乘法时，4×4矩阵乘法每个周期要花几十微秒；换上mat_mul_q15()后，直接降到几微秒。姿态更新率从1kHz提升到了5kHz。这大概就是优化的乐趣所在。

如果你对定点数学库感兴趣，欢迎关注我。这个系列到这里已经覆盖了 sin()、atan2()、sqrt()、exp()、log() 和矩阵乘法，基本覆盖了嵌入式中最常用的数学运算。后续如果大家感兴趣，我可以继续写定点矩阵求逆、定点卡尔曼滤波或者定点神经网络推理。