卡尔曼滤波器族在感应电机状态估计中的MATLAB实现与对比
2026/9/9 3:21:36
去年年底给一个工业振动监测模块做固件升级，主控从Cortex-M3换到Cortex-M7，顺便把跑在系统里的FFT、FIR、RMS这一整条信号链全部切换到Arm官方CMSIS-DSP库上。切换本身并不复杂，真正费时间的是把库的源码从头到尾审计了一遍——那套固件要过认证，关键算法必须有人看得懂、说得清、验证得了。也是在这个过程中，我对CMSIS-DSP的理解从“调API”上升到了“看门道”，所以这篇文章与其说是评测，不如说是一份源码审计笔记加工业落地踩坑记录，适合正在做嵌入式信号处理，或者准备把DSP库搬进量产固件的工程师参考。
1. 架构全景：先搞清楚CMSIS-DSP到底是怎么设计的
1.1 为什么嵌入式信号处理绕不开这个库
CMSIS-DSP是Arm提供的一套面向Cortex-M系列内核的官方DSP函数库，覆盖了从基本加减乘除到FFT、FIR、矩阵运算、统计函数、PID控制甚至部分机器学习算子。它的意义在于：让你在写嵌入式信号处理代码时，不需要从头撸FFT或者FIR，直接调官方优化过的接口就行，而且能自动利用Cortex-M的FPU、DSP扩展指令以及新内核上的Helium（MVE）向量扩展。
我这些年接过不少工业项目，碰到过很多工程师对这套库的态度两极分化：一种是什么都敢用但从来不看源码，出了问题只能盲猜；另一种是什么都不敢用，宁可自己写一套凑合的滤波器。这两种都有问题。官方的库不是黑盒，里面的实现路径基本就是ARM指令集优化的最佳实践，把它吃透以后，你不仅能正确调用，还能知道什么时候应该自己写、什么时候必须用库、出了问题第一眼该往哪里排查。
从源码审计的角度看，这套库还有一个非常值得研究的点：它是ARM架构指令集能力最集中的展示场。同一套算法，它往往会同时提供纯C版本、ARM DSP指令优化版本、MVE优化版本，通过预处理宏在编译期切换。你读一份源码，等于同时看到了不同硬件能力层级下的算法落地思路，这对做性能敏感型固件的人来说价值极大。
1.2 源码目录看完之后的几点印象
拿到CMSIS-DSP源码后，第一件事不是去读某个具体函数，而是把目录结构过一遍。以CMSIS 5.x版本为例，核心代码在CMSIS/DSP目录下，分成了Include和Source两大部分。
Include下面最核心的是arm_math.h，这个头文件把所有对外API、实例结构体、编译控制宏、数据类型定义全部汇总了。很多人在工程里只include这一个头文件就够了，但它内部会根据宏定义再拉取arm_math_types.h、arm_math_memory.h以及dsp子目录下分类的头文件。
Source目录则按功能模块划分得清清楚楚，我整理了一张主要模块清单：
| 模块目录 | 典型函数 | 工程用途 |
|---|---|---|
| BasicMathFunctions | arm_add_f32、arm_mult_q15 | 向量加减乘、缩放、偏移 |
| TransformFunctions | arm_cfft_f32、arm_rfft_fast_f32 | FFT、DCT、MFCC等变换 |
| FilteringFunctions | arm_fir_f32、arm_biquad_cascade_df1_f32 | FIR、IIR、LMS自适应滤波 |
| MatrixFunctions | arm_mat_mult_f32、arm_mat_inverse_f32 | 矩阵乘、转置、求逆、Cholesky |
| StatisticsFunctions | arm_mean_f32、arm_rms_f32、arm_std_f32 | 均值、RMS、标准差、方差 |
| ComplexMathFunctions | arm_cmplx_mag_f32 | 复数模值、乘加、点积 |
| ControllerFunctions | arm_pid_f32 | PID控制、电机控制 |
| SupportFunctions | arm_fill_f32、arm_copy_f32 | 缓冲区填充、复制、类型转换 |
| CommonTables | armTwiddleCoef、armBitRevTable | 旋转因子表、位反转表 |
这个表可以帮助新手快速定位自己想要的功能在哪。审计源码的时候我建议先看CommonTables，再看TransformFunctions，因为两者耦合最深，FFT的性能往往由表结构和访存方式决定。其他像DistanceFunctions、SVMFunctions、BayesFunctions是近几个版本为机器学习场景新增的模块，工业固件里一般用不到，但如果你做预测性维护，跑分类器时可能会用到。
读源码的过程中有一处印象很深：这套库对“代码复用”的处理方式不是把所有东西都堆在一个文件里，而是把公共的蝶形运算、位反转、基础数学操作拆成内部函数，放到PrivateInclude或模块内的静态函数里。好处是每个对外函数都足够薄，坏处是如果你只看某个.C文件而不看它调用的内部函数，很容易漏掉关键逻辑。审计的时候一定要顺着调用链往下挖。
1.3 新旧API的暗流：从radix-4到cfft的演进
如果你接手过一个跑了两三年的老固件，大概率会在代码里看到arm_cfft_radix4_f32、arm_cfft_radix4_init_f32这种老接口。它们是CMSIS-DSP早期版本的主力API，设计思路是每种变换单独搞一个实例结构体和初始化函数，radix4就是按4点一组的蝶形做基4分解，radix2就是基2分解，还有radix8。新版本则把接口统一到了arm_cfft_f32，配合一个arm_cfft_init_f32来初始化实例。
这个API演进不是单纯的改名，背后是算法实现的整体重构。老的radix4接口对FFT长度有限制，要求是4的幂或者特定的2的幂；新的arm_cfft_f32支持将长度分解成2、3、4、5的混合基组合，灵活性高很多。内部还会根据长度自动选择最合适的小基数蝶形组合，不再需要开发者自己去选radix几。
对固件工程师来说，新旧API最大的影响是数据结构变了。老接口把ifftFlag和bitReverseFlag直接塞进初始化函数，新接口则是在每次变换调用时传入。这意味着如果你把老工程直接平移到新库上面，很多调用点都要改。我在审计时特意对比过，新库为了兼容老用户，部分文件里还保留了旧接口的实现，但头文件已经不再主动推荐了。遇到老代码，我一般建议一次性迁移到新API，不要左右横跳。
2. 源码审计：核心算子实现里的工程细节
2.1 FFT实现：从表驱动蝶形到缓存友好的数据排布
FFT是整个库的灵魂，也是最值得花时间读的源码。以新版arm_cfft_f32为例，整个流程可以拆成三块：初始化时查表选base、蝶形运算、位反转重排。
先说旋转因子表。CMSIS-DSP把不同FFT长度需要用的旋转因子预计算好了，放在CommonTables目录下，命名为armTwiddleCoef_16、armTwiddleCoef_32这样的常量数组。每次做FFT时，不是现场用三角函数去算旋转因子，而是直接从表中取。这个设计的直觉很简单：三角函数计算在Cortex-M上很贵，查表是经典的空间换时间。审计时我确认过一个细节，表里的值全部是float32或者q31格式预生成好的，所以即使目标芯片没有FPU，只要用定点版本，旋转因子也不会引入额外计算误差。
再看蝶形运算。源码里一个典型的radix-4蝶形会一次处理四个输入点，做三次复数乘法和若干次加减法，然后结合旋转因子的实部虚部进行交叉运算。读这段代码时你一定要注意复数乘法的展开方式：浮点复数乘法a+bi乘以c+di，结果是(ac-bd)+(ad+bc)i，编译器不一定能自动优化成最少乘法，但CMSIS-DSP的源码里是手动展开的，因为DSP库和普通C代码不一样，它默认就是按照ARM FPU指令特性来组织的。
位反转这一步在arm_bitreversal32里实现。FFT的蝶形输出顺序是乱序的，必须把数据按位反转后的索引重新排一遍才能得到正确的频域顺序。老版本里位反转是单独的循环，新版本则把位反转和旋转因子表的定位合并在实例结构体中，处理时直接按表走，减少了一次索引计算的额外开销。
审计FFT源码时，我比较关注数据排布的缓存友好性。Cortex-M7有I-Cache和D-Cache，如果数据访问是跳跃式的，缓存命中率会很惨。CMSIS-DSP的FFT在处理较长长度时，尽量让蝶形运算访问相邻区间，而不是一下子跨到内存远端。这不一定能在源码里直接看到，但你在不同平台上做耗时对比时，能明显感觉到同样的算法在不同数据排布下差距很大。
2.2 FIR和IIR：状态缓冲区的管理是真正的功力
滤波是工业信号处理里最日常的需求。CMSIS-DSP的FIR函数看起来很简单，但源码里有一个非常容易被忽略的设计：状态缓冲区的大小和更新方式。
先看初始化。arm_fir_init_f32原型大概是这个样子：
void arm_fir_init_f32( arm_fir_instance_f32 * S, uint16_t numTaps, const float32_t * pCoeffs, float32_t * pState, uint32_t blockSize);官方头文件注释里明确要求pState缓冲区大小必须是numTaps + blockSize - 1。很多人在初始化时习惯只分配numTaps个元素，结果函数一跑，数据越界，轻则波形不对，重则hardfault。为什么是numTaps + blockSize - 1？因为FIR处理是按块（block）来做的，每次处理blockSize个样本时，除了要当前块的样本，还需要前numTaps-1个历史样本，所以状态区必须能同时放下历史值和当前块数据。
更关键的是处理完一块数据之后，源码会把状态缓冲区尾部的数据移到头部，保证下一块处理时历史数据是连续的。如果你把源码从头读一遍，会发现arm_fir_f32主循环结束后，有一段循环专门做状态更新，把最新blockSize个样本搬到pState开头。这就是为什么初始化和每次处理时的blockSize必须保持一致，否则状态搬迁逻辑就乱了。
IIR的实现同样有类似设计，只是它用状态数组保存前几拍的中间变量。审计时要特别注意不同IIR结构（直接I型、直接II型、转置型）的状态数量不一样，比如biquad级联结构的每个二阶节要保存两个状态量，初始化和复位时清零顺序不能错。
我在审计这些滤波器源码时有一个很深的体会：官方的代码把“块处理”和“流式处理”的边界处理得非常好。它默认你是一次处理一个block的数据，然后在中断里反复调用。如果你误以为它是样本级处理接口，每个样本调一次，性能会非常难看，而且状态更新逻辑也不对。
2.3 矩阵与统计函数的定点设计和溢出边界
工业固件里还有一个常见需求是矩阵运算和统计特征计算。CMSIS-DSP的矩阵函数支持浮点和定点。浮点版本比较简单，arm_mat_mult_f32内部对矩阵乘法做了循环展开，配合ARM_MATH_LOOPUNROLL宏还能进一步优化。
定点版本才是真正的雷区。以arm_mat_mult_q15为例，输入是Q15格式，但内部累加时用的是Q31甚至Q63精度的累加器。原因很简单：多个Q15乘法加一起，结果位数会快速膨胀，如果每步都截断回Q15，误差会大到不可接受。所以源码的策略是：中间累加不截断，直到最后输出时才按目标格式饱和截断。审计时我看到内部累加器用了类似SMLALD这种带符号长乘累加指令，说明它就是为ARM DSP扩展指令定制的。
统计函数里arm_rms_f32是另一个经常被误用的地方。它的计算流程是先求平方、再求均值、最后开方，整个过程在浮点下很直观。但arm_rms_q15版本输出就不是Q15了，而是经过缩放处理后的格式。很多工程师拿Q15版本直接用浮点思维去解读结果，算出来的RMS值跟万用表对不上，第一反应是库有bug，实际上是对Q格式的还原逻辑不对。
我审计完矩阵和统计函数后，给团队的建议是：没有FPU的M0/M3平台，能用浮点尽量用浮点但要注意仿真时间，实在不行才上定点；有FPU的M4/M7平台，优先浮点版本，因为浮点单元已经把大部分日常计算扛住了，定点版本省下的那点时间可能不够填补开发和调试成本。
3. 工业固件落地：从源码选择到产线运行
3.1 工程集成与编译宏的正确姿势
把CMSIS-DSP加进工程有两种常见方式：源码方式直接把需要的.C文件拖进工程编译，或者用预编译的库文件。我倾向于源码方式，因为这能配合我之前的源码审计思路，按需取用、方便裁剪。库文件虽然省事，但在裁剪和特定编译选项上不够灵活。
集成时第一步是确保arm_math.h能正常包含。它内部依赖cmsis_compiler.h和core_cmX.h，所以你的工程里必须有对应的CMSIS-Core头文件。如果你用的是STM32CubeMX生成的工程，CMSIS-Core头文件已经在Driver层里了，CMSIS-DSP的Include路径加进去就行。
接下来是编译控制宏。下面这个表是我反复用到的一个清单：
| 宏定义 | 作用 | 建议 |
|---|---|---|
| ARM_MATH_CM4 / ARM_MATH_CM7 | 指定Cortex-M内核，老版本必须定义 | 新库大多能自动识别，但为了兼容老代码建议保留 |
| ARM_MATH_DSP | 启用DSP扩展指令优化 | M3/M4/M7可用，M0/M0+不要开 |
| ARM_MATH_LOOPUNROLL | 启用循环展开优化 | 追求性能时开，代价是代码体积增大 |
| ARM_MATH_MVEI / ARM_MATH_MVEF | 启用Helium整数/浮点向量扩展 | 仅Cortex-M55、M85等MVE内核可开 |
| ARM_MATH_BIG_ENDIAN | 大端模式 | 一般小端平台不定义 |
| ARM_MATH_MATRIX_CHECK | 矩阵函数做维度检查 | 调试期开，量产可关闭以省时间 |
| ARM_MATH_ROUNDING | 定点运算启用舍入模式 | 对精度有要求时开 |
| ARM_DSP_CONFIG_TABLES | 启用FFT表的按需裁剪 | 固件大小敏感时强烈建议研究这项 |
编译优化等级上，我一般用-O2起步，性能不够再试-O3。但有个红线：绝对不要随便给整个工程加-ffast-math。这个选项会改变浮点运算语义，把IEEE754的一些边界行为禁掉，CMSIS-DSP浮点版本在某些边界条件下会算出错误结果，排查起来极其痛苦。我在调试一个长时间运行的固件时被这个问题坑过一次，从那以后FFT、RMS这类算子的编译优化选项都和业务代码分开设。
3.2 按需裁剪：别让固件背着整个库跑
很多工程师第一次把CMSIS-DSP加进工程后，烧录一看固件大了几十KB，立刻觉得库太肥。其实库本身是模块化的，你编译多少源文件就进多少代码，不会把整个库全编进去。真正容易悄悄变大的是各种查表常量。
以FFT为例，如果你用arm_cfft_f32，它默认会带上很多长度对应的旋转因子表和位反转表。如果不做任何裁剪，这些常量表加起来可能有好几KB甚至十几KB。CMSIS-DSP从某个版本开始引入了ARM_DSP_CONFIG_TABLES机制，允许你通过宏定义只保留需要的FFT长度表。
我做过一个实际案例：固件只需要1024点FFT，裁剪前常量表加上各种支撑代码大概占了13KB Flash，裁剪后只剩下旋转因子表、位反转表和少量初始化逻辑，整体少了大概7KB。做法是定义ARM_DSP_CONFIG_TABLES，然后用arm_fft_bin_data这个全局结构体指定需要的表：
#define ARM_DSP_CONFIG_TABLES #define ARM_FFT_ALLOW_TABLES #define ARM_FFT_ALLOW_1024_POINT具体宏的名字会随版本变化，但思路是一致的：告诉编译器“我只要1024点的表”，它就不会把16点、32点、64点、512点这些用不到的常量编进来。需要注意，如果改了裁剪配置，一定要重新初始化实例结构体，否则运行时表索引对不上内存布局，FFT结果会变成乱码。
裁剪这条线做完以后，我通常还会评估RAM占用。以1024点浮点FFT为例，输入缓冲区是实部虚部交替的数组，需要210244字节，也就是8KB。如果你用的是arm_rfft_fast_f32做实数FFT，仍需分配2倍长度的缓冲区，因为后一半要留给内部虚部处理。这些在做内存规划时就要算清楚，别等到跑起来才发现RAM不够。
3.3 内存对齐、DMA与缓存同步的协同设计
CMSIS-DSP对缓冲区对齐有要求，官方头文件里随处可见__ALIGNED(4)或者类似的声明。基础要求是4字节对齐，这在大部分MCU工程里很常见。但如果你用了MVE指令，也就是Cortex-M55这类带Helium的内核，对齐要求会提高到16字节，因为MVE的向量加载要求对齐访问性能更好，不对齐时虽然也能工作，但可能触发异常或导致性能断崖式下跌。
更常见的问题是DMA和缓存同步。在M7核上，如果DMA把ADC数据直接写到内存，然后CPU用CMSIS-DSP去读，中间隔着一个D-Cache，就会有一致性问题：DMA写完了内存，但缓存里还是旧数据，CPU一读全是无效值。解决办法是在DMA传输完成中断里做Cache Invalidate，在CPU写完数据准备交给DMA外发时做Cache Clean。
我现在的做法是固定一套模板：
// DMA写完后，CPU读取前 SCB_InvalidateDCache_by_Addr((uint32_t *)adcBuf, sizeof(adcBuf)); // CMSIS-DSP处理完成后，准备用DMA搬走结果前 SCB_CleanDCache_by_Addr((uint32_t *)resultBuf, sizeof(resultBuf));这条经验几乎每次都会被用到。很多工程师在M7上跑FFT出乱码，第一反应是算法不对，查了半天最后发现是D-Cache没处理。
4. 常见问题与排查技巧实录
4.1 一份可以直接抄的避坑速查表
下面是我在实际项目中整理的一张速查表，基本覆盖了CMSIS-DSP最常见的坑：
| 现象 | 常见原因 | 处理方法 |
|---|---|---|
| 编译报错找不到core_cm7.h | CMSIS-Core路径没配全 | 把CMSIS/Core/Include加入头文件路径 |
| 调用FFT后结果全是NaN | 输入缓冲区里有非法浮点值 | 检查ADC原始数据转浮点的转换过程 |
| FFT幅值偏大很多倍 | 浮点FFT没有归一化 | 幅值谱分析时按1/N或1/sqrt(N)处理 |
| FIR输出出现周期性毛刺 | pState空间不够或未清零 | 重建缓冲区，严格按numTaps+blockSize-1分配 |
| 定点FIR输出噪声底抬升 | Q格式缩放选择不对 | 检查输入、系数、输出三段Q格式并统一 |
| 切断DMA后数据正常，接DMA后FFT乱码 | D-Cache一致性问题 | 在DMA和CPU之间做Clean/Invalidate |
| 裁剪FFT长度表后结果异常 | 实例结构体重用旧表指针 | 裁剪配置改动后必须重新调用init函数 |
| 中断里调用库函数卡死 | 库函数执行时间超过中断周期 | 把长块运算放到主循环，中断只做数据搬运 |
| 加了ARM_MATH_LOOPUNROLL后Flash不够 | 循环展开导致代码膨胀 | 关闭该宏或只对热函数单独编译优化 |
| 不同编译器结果有细微差别 | 浮点运算顺序和优化选项不同 | 以IEEE双精度/单精度为准做一致性测试 |
这张表不是理论推测，每一条都是我自己或同事线上踩过然后定位过的，拿去对照基本能省半天排查时间。
4.2 案例一：振动谱里出现“镜像频谱”
有次帮一个做旋转机械状态监测的客户排查问题，设备采集轴承振动信号，采样率10kHz，FFT点数1024，理论分辨率大概9.77Hz。固件是用老版本CMSIS-DSP的arm_cfft_radix4_f32写的，频谱图上在某个频率左右出现了一个原本不存在的对称波形，看上去像镜像。
排查过程很有意思。第一反应是混叠，于是查抗混叠滤波器，发现前端RC滤波带宽足够；第二反应是采样率配置错，但用示波器数了PWM触发间隔，频率是对的；最后静下心审计代码，发现他把实采样数据直接填进了复数FFT的实部数组，虚部全部置零。这里就有个细节：如果你用复数FFT处理实数信号，频谱天然是对称的，正负频率都会出现，而且幅度是真实幅度的一半。如果只取前一半看正频谱，确实也可能得出怪异结论。
后来换成arm_rfft_fast_f32做实数FFT，因为实FFT在输出时已经处理好了只输出N/2个有效频点，范围就是0到奈奎斯特频率，既省了一半计算量，也避免了这种人为制造的“镜像”。这个案例给我的教训是：先选对算法路径，再谈优化，不然源码审计做得再细也是在错误路上打转。
4.3 案例二：定点FIR输出的噪声底抬升
另一个案例是在一颗不带FPU的Cortex-M0+上做电流信号滤波。采样值用Q15格式，滤波器系数用Q15，FIR输出直接转成Q15回写给DAC。现场运行一时半会儿看不出大问题，但放到频谱仪上一测，噪声底比预期抬高了将近20dB。
排查下来问题出在累加截断策略。Q15的乘法结果是Q30量级，两个Q15相乘加到累加器里，如果用Q31累加，多次累加以后小数位对齐其实还好，但如果每次都把中间结果直接右移截断回Q15，误差就会被累积放大。CMSIS-DSP定点FIR内部累加是整word宽度的，但如果你自己写了类似“acc = (acc >> 15)”这种代码，就很容易埋雷。
最后我们把滤波器改成库自带的arm_fir_q15，并且调整了系数Q格式，让系数尽量满量程从而减小量化噪声，噪声底才降回合理范围。这里特别想强调：定点设计不能只看头尾格式，中间的累加位宽、饱和处理、缩放时机都决定了最后的信噪比。
4.4 测性能别拍脑袋，我都是这么量化的
做固件落地，性能评估不能靠体感。我推荐用Cortex-M内核自带的DWT周期计数器来测函数耗时，方法如下：
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; DWT->CYCCNT = 0; DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; uint32_t t0 = DWT->CYCCNT; arm_cfft_f32(&fftInst, fftBuf, 0, 1); uint32_t t1 = DWT->CYCCNT; // 耗时 = t1 - t0 个CPU周期这样测出来的时间不仅包括算法本身，还包括访存开销。我通常在关中断情况下连续跑多次取最小值，避免中断打扰。测完后换算成时间，比如主频400MHz，一个周期2.5ns，10000个周期就是25us，就能判断实时性是否满足控制周期或者采样周期要求。
5. 写在最后
读CMSIS-DSP源码这件事，我在不同项目里重复过好多次，每次都有新收获。官方库不是银弹，它也有它自己的设计假设，比如假设你按块处理数据、假设你对齐缓冲区、假设你理解了Q格式再动手。这些假设都写在源码里，读一遍比看十遍文档都管用。
我个人在工业固件项目里的体会是：不要神化官方库，也不要轻视它。盲目调用，出了问题你连怎么描述都描述不清；完全自己写，等于放弃ARM指令集优化和社区验证。正确的姿势是，把CMSIS-DSP当成一个可靠的算法底座，但你对底座下面的每根柱子都心里有数。
如果你接下来打算在项目里引入或升级CMSIS-DSP，我的建议是先挑一两个你最常用的函数，顺着源码把初始化结构体、核心循环、状态更新这段链路读通，然后基于我的速查表做一轮针对性测试。这套工作做完，再复杂的信号链你也有底气去调了。