矩阵这个东西我见过太多RM电控新队员在第一次翻卡尔曼滤波推导的时候被劝退——满屏的加粗大写字母、上标转置、下标k还有那个看着就不想动的(PHᵀ(HPHᵀR)⁻¹)。但真正卡住人的从来不是卡尔曼滤波本身而是它默认你已经熟练的矩阵语言。我们中科大战队在带新人的时候有个不成文的规矩谁想碰卡尔曼先花两周把矩阵分析的基本操作过一遍能自己手推一遍增益公式再说。这篇就聊聊为什么卡尔曼滤波之前必须先啃矩阵矩阵分析里哪些东西是电控真正用得到的以及怎么把纸上的矩阵公式落到STM32的代码里还不炸。适合刚进RM电控组、听说过卡尔曼但没敢下手的同学也适合任何想把状态估计从抄库变成自己写的人。1. RM电控里卡尔曼滤波到底在算什么1.1 陀螺仪漂移和加速度计噪声是一对天生的冤家先说清楚我们为什么要用卡尔曼。RM的云台和底盘上姿态估计几乎都绕不开IMU。陀螺仪的好处是短时间响应快、动态准坏处是积分之后会有零偏累积你放那儿不动一分钟之后角度能飘出去好几度。加速度计正好相反静态的时候它能给你一个绝对的重力方向参考但机器人一跑起来、电机一抖、底盘一颠加速度计输出的就不只是重力了高频噪声大得没法直接用。这两者一个信短期一个信长期单独用都不行。卡尔曼滤波干的事情本质就是给这两个传感器的输出各自配一个信任度然后按最优的方式加权融合。这个信任度在数学上就是方差和协方差写成矩阵就是协方差矩阵。你如果只学过一维的加权平均脑子里想的是陀螺仪权重0.98加速度计权重0.02这在一维场景下勉强能跑。但真实的姿态估计里状态往往不只一个角度还有角速度、零偏观测也不只一路这时候权重就从一个数变成了一整个矩阵也就是卡尔曼增益K。所以第一层认知要先立起来卡尔曼滤波不是两路数据加权平均这么简单它是多维状态在多维观测下的最优估计。而多维这两个字就注定了你必须用矩阵来描述状态、描述不确定性、描述观测关系。1.2 标量思维的天花板在哪里我见过有同学写了个一维卡尔曼用来融合单个轴的陀螺仪和加速度计跑得还挺顺然后他试图把这个代码复制三份去处理roll、pitch、yaw三个轴。结果yaw轴死活收敛不了因为yaw没有绝对参考加速度计在水平旋转时给不出任何yaw信息三个轴之间存在耦合而他的代码里三个轴是独立跑的完全没考虑耦合。这就是标量思维的天花板。当你把三个轴独立处理的时候你隐含假设了三个轴的误差是互不相关的也就是协方差矩阵是个对角阵。实际情况里传感器误差、坐标系旋转、动力学耦合都会让这个矩阵出现非对角项。非对角项一出现标量那套加减乘除就彻底不够用了你必须上矩阵。还有一个更隐蔽的点坐标系变换。陀螺仪测的是机体系下的角速度但你要的是世界系下的姿态角。从机体系到世界系的转换就是一堆旋转矩阵相乘。旋转矩阵是矩阵分析里最基础的对象之一正交、行列式为1、转置等于逆这些性质你如果没在矩阵课上学过看姿态解算代码的时候会一头雾水——为什么这里要转置为什么那里要乘逆完全靠背。2. 卡尔曼滤波前必须打通的五个矩阵概念2.1 矩阵本质上是一个线性变换别只当成数字表格很多教材一上来就讲矩阵是数字排成的矩形阵列然后开始讲加减乘除。这种讲法对考试有用但对理解卡尔曼没用。你脑子里要建立的第一个直觉是矩阵是一个把向量从一个空间搬到另一个空间的变换。举个例子一个3×3的旋转矩阵R乘上一个表示机体角速度的三维向量得到的就是世界系下的角速度。这个矩阵不产生新信息它只是换了描述问题的坐标系。理解了这一点你就能明白为什么卡尔曼的预测方程里状态转移矩阵F乘的是状态向量x——它在描述上一时刻的状态经过系统的动力学变成了这一时刻的状态而这个变成的过程是线性的所以能用矩阵表示。矩阵作为线性变换还带来几个重要推论。可逆的矩阵对应一一对应的变换不可逆或者接近不可逆的矩阵对应信息丢失的变换。卡尔曼滤波里那个求逆操作(HPHᵀR)⁻¹本质就是在问这个观测能提供多少新信息如果这个矩阵接近奇异说明观测几乎提供不了新信息数值上就会抖。后面讲数值稳定性的时候会再回来说这一点。另外要习惯的符号约定一般用大写粗体或者大写字母表示矩阵如F、H、P小写粗体表示向量如x、z斜体小写表示标量。元素用双下标比如a_ij表示第i行第j列。这套约定不是装饰它能让你在读推导的时候一眼分清哪些是标量运算、哪些是矩阵运算。转置用上标ᵀ逆用上标⁻¹这两个符号在卡尔曼公式里出现的频率高到你必须形成肌肉记忆。2.2 矩阵乘法、转置与逆运算规则背后的物理含义矩阵乘法是卡尔曼滤波里用得最多的运算。规则你肯定知道A(m×n)乘B(n×p)得到C(m×p)C的元素是A的行点乘B的列。但我要强调的是维度匹配这个习惯。新手最常犯的错误就是矩阵维度对不上然后对着报错发呆。卡尔曼的预测方程 P⁻ FPFᵀ Q你数一下维度如果状态是n维F是n×nP是n×nFᵀ也是n×nQ是n×n。全对上了这个方程的物理含义是不确定性的传播——系统演化会把原来的不确定性F和P各拉伸一次再加上过程噪声Q带来的新增不确定性。矩阵乘法不满足交换律AB不等于BA这一点在推导卡尔曼的时候非常关键。为什么更新方程里是K(z-Hx̂)而不是(z-Hx̂)K为什么先算K再算x̂顺序不能乱。有同学写代码的时候把乘法顺序搞反了编译器不报错因为维度刚好凑得上但结果就是错的。我的建议是写矩阵代码的时候每个乘法后面用注释标出维度比如// F(n×n) * x(n×1) (n×1)这个习惯能帮你省下大量调试时间。转置的物理含义在协方差矩阵上体现得最明显。因为协方差矩阵是对称的P等于Pᵀ所以你看到FPFᵀ这个形式就可以理解为把F方向的变换同时施加在协方差矩阵的两侧保证结果的对称性。如果只写FPF结果就不对称了那它就不再是一个合法的协方差矩阵。逆矩阵是卡尔曼里的重操作。理论上你会写(A)⁻¹但代码里几乎从来不会真的去求逆。原因有两个一是求逆数值不稳定尤其是矩阵接近奇异的时候二是求逆的复杂度高在单片机上用不起。实际做法是把式子K P⁻Hᵀ(HP⁻HᵀR)⁻¹改写成解线性方程组的形式。设S HP⁻HᵀR那么看K P⁻HᵀS⁻¹等价于解SᵀKᵀ (P⁻Hᵀ)ᵀ或者更常见的是用Cholesky分解把S拆成LLᵀ然后前代回代求解。这就是后面代码实现里的重点。2.3 行列式、秩与可逆性决定滤波器崩不崩行列式在卡尔曼代码里你几乎不会直接算但它是理解为什么矩阵会病态的关键。一个方阵的行列式为0意味着它不可逆也意味着它对应的线性变换把空间压扁了——比如把一个三维空间压成一个平面。在卡尔曼滤波里如果观测矩阵H把不同状态压到了同一个观测上比如两个状态都只体现在同一个观测量里那你的观测就提供不了足够信息去区分这两个状态滤波器就会发散。秩刻画的是矩阵有效维度。一个n×n矩阵如果秩小于n就说明它的列向量线性相关变换之后有信息丢失。落到卡尔曼上状态转移矩阵F如果降秩意味着系统的某些状态会消失那预测就没意义了。观测矩阵H如果降秩意味着观测不足以支撑状态估计。这里有个实操经验拿到一个滤波发散的问题先去看H矩阵和F矩阵的秩再去调Q和R。我见过太多人一遇到发散就疯狂调Q调了半天治标不治本问题其实出在模型本身——H矩阵设置得让某个状态永远观测不到那这个状态的不确定性就会无限增长怎么调参都救不回来。举个RM里常见的例子。你做云台yaw轴估计状态设成[角度, 角速度]观测只有陀螺仪的角速度那么H [0, 1]。这时候角度这个状态完全没有观测。如果你不做任何处理纯靠F积分角度会很快飘走协方差矩阵P的角度对应项会线性增长到很大。这不是滤波器的bug而是你没有提供角度的绝对参考这个物理事实在矩阵上的体现。2.4 协方差矩阵与正定性不确定性怎么用矩阵描述协方差矩阵是卡尔曼滤波的灵魂但很多人对它的理解只停留在对角线是方差这一层。对角线元素确实是各个状态的方差但非对角元素才是关键它描述的是状态之间的相关性。比如你的速度估计偏高往往角度估计也会偏高这种相关性就体现在协方差矩阵的非对角项上。协方差矩阵有两个必须记住的数学性质对称以及半正定严格说在满秩情况下是正定。正定的定义是对于任何非零向量x都有xᵀPx 0。这个性质不是数学家闲着没事定的它有实实在在的物理意义——不确定性在任何方向上的投影都必须是正的不能有负的方差。一旦你的代码算出了一个非正定的P说明数值出问题了滤波器马上就要崩。正定性在卡尔曼里还和Cholesky分解直接挂钩。Cholesky分解要求矩阵正定你把S HPHᵀR用Cholesky拆成LLᵀ的前提就是S正定。而S是不是正定理论上H、P、R都是正定或者半正定的加起来的S是正定的所以能用Cholesky。但如果数值误差让P失去了正定性Cholesky就会失败或者出NaN。这就是为什么很多卡尔曼实现里要有重正定化的兜底操作。那么协方差矩阵在实际标定时怎么理解P的初始值大表示我一开始对状态一无所知P的初始值小表示我一开始就挺确定。P会随着预测变大不确定性增加随着更新变小观测带来了信息。你看P的大小就能判断滤波器当前自信不自信。调试的时候把P的对角线打印出来比看状态估计值有用得多。2.5 矩阵求导是卡尔曼增益公式的真正来源前面讲的都是会用这一节讲为什么对。卡尔曼滤波的更新公式x̂ x̂⁻ K(z-Hx̂⁻)里的K不是拍脑袋定的它是最小化估计误差协方差推导出来的最优点。推导过程要用到矩阵求导这是很多人跳过的一步但跳过之后你对整个滤波器的理解就浮在表面。核心要掌握的求导公式不多最常用的两个一是标量对向量求导∂(aᵀx)/∂x a二是二次型对向量求导当A对称时∂(xᵀAx)/∂x 2Ax。卡尔曼增益的推导里要把后验误差协方差的迹trace对K求导并令其为零过程中会反复用到二次型的导数公式。还有一个工具是矩阵微分恒等式尤其是涉及迹的那些∂tr(AB)/∂A Bᵀ这类。你不需要背全部但要能查得到、看得懂。我的建议是第一次推导的时候照着教材慢慢推一遍把每一步用到哪个公式标注在旁边。推完一遍之后你会发现卡尔曼增益那个看着吓人的(PHᵀ(HPHᵀR)⁻¹)其实就是一个观测信息与先验信息之比的矩阵版本。史荣昌那本《矩阵分析》和《矩阵分析引论》里求导相关的章节偏理论符号约定也偏数学。看得头晕是正常的。我的经验是把它当成工具书——需要哪个恒等式去查哪一节不要试图一次性从头读到尾。真正让你理解卡尔曼求导的是你在纸上跟着推一遍而不是读十遍别人怎么推。3. 把矩阵公式落到嵌入式代码里3.1 五个滤波公式的矩阵对照先把卡尔曼的五个公式用矩阵形式列清楚这是后面写代码的蓝图。状态向量x是n×1观测向量z是m×1状态转移矩阵F是n×n观测矩阵H是m×n过程噪声协方差Q是n×n观测噪声协方差R是m×m协方差矩阵P是n×n卡尔曼增益K是n×m。步骤公式维度状态预测x̂⁻ F x̂ B un×1协方差预测P⁻ F P Fᵀ Qn×n计算增益K P⁻Hᵀ(HP⁻HᵀR)⁻¹n×m状态更新x̂ x̂⁻ K(z − H x̂⁻)n×1协方差更新P (I − KH)P⁻n×n维度对不上的地方一眼就能看出来。写代码的时候我强烈建议把这五步分别封成函数不要让它们揉在一个大函数里。原因很实际调参和调试的时候你要单独打印某一步的中间量分开了才好插断点和打印。状态更新里那个(z − Hx̂⁻)叫新息也就是观测值和预测的观测值之间的差。这个量的物理含义是预测错在哪。如果新息一直很大说明你的模型F、H有问题如果新息抖动得厉害说明R设小了如果新息长期偏一边说明状态量或者模型有系统性偏差。调试卡尔曼盯着新息比盯着状态输出更有用。3.2 单片机上的小矩阵库怎么写才不出错RM的板子基本都是STM32跑裸机或者RTOS没有numpy这种库。你需要一个轻量的矩阵库。有人会去搬DSP库或者其他开源库但对于入门阶段我建议自己写一遍因为写的过程就是理解维度的过程。关键设计原则有几条。第一通信用静态数组不用动态内存。n和m在编译期就确定比如状态6维、观测3维用float A[6][6]这种形式避免malloc带来的碎片和不确定性。第二函数只处理确定的维度别为了通用性写成带循环变量维度的形式单片机上的通用性不值那点性能牺牲。第三对单位矩阵、转置、矩阵乘这几个高频操作单独优化。下面给一个六维状态下的核心操作示例用C写#define N 6 typedef float mat_t[N][N]; void mat_mul(const mat_t A, const mat_t B, mat_t C) { for (int i 0; i N; i) for (int j 0; j N; j) { C[i][j] 0.0f; for (int k 0; k N; k) C[i][j] A[i][k] * B[k][j]; } }这个三层循环是标准写法但在单片机上有优化空间。比如循环顺序可以改成i-k-j让内层循环连续访问内存缓存命中率更高。不过对于6维这种小矩阵优化的收益不大保持代码清晰更重要。等你的状态维度大到十几维、控制周期又很短的时候再考虑用DSP指令或者查表优化。求逆那块不要用高斯-约当消元去求通用逆用Cholesky分解解方程更稳。Cholesky的代码也不长三十行左右网上有成熟参考。关键是它在S正定的时候过程稳定而且分解完一次可以复用来做前代回代。3.3 F、H、Q、R四个矩阵的构造实操与参数选择模型矩阵的构造决定了滤波器能不能用。以RM云台姿态估计里最常见的角度角速度模型为例状态x [θ, ω]ᵀ观测是陀螺仪角速度z ω_gyro。状态转移矩阵F按匀速模型近似θ(k) θ(k−1) ω(k−1)·dtω(k) ω(k−1)所以// dt 单位为秒 F[0][0] 1.0f; F[0][1] dt; F[1][0] 0.0f; F[1][1] 1.0f;观测矩阵H其实只用观测角速度H [0, 1]。过程噪声Q反映模型的不可信程度。匀速模型忽略了角加速度所以角度方程的不确定性来自角加速度。工程上常用的Q构造方式是取一个加速度噪声参数q角度项为q·dt³/3速度项为q·dt交叉项为q·dt²/2。你可以先不纠结这个公式怎么来的先用经验值调看收敛效果。观测噪声R就是陀螺仪读数的方差。这个可以在静止状态下采几百个点直接算方差是个很实在的初始化方法比瞎猜好得多。// 静止采样计算 R float mean 0, var 0; for (int i 0; i 500; i) mean data[i]; mean / 500; for (int i 0; i 500; i) var (data[i]-mean)*(data[i]-mean); var / 500; R[0][0] var;注意R的标定一定要在机器人静止、电机不转、没有外力干扰的条件下做。如果标定的时候云台在抖动算出来的方差会偏大滤波器就会过度信任模型、不够信任观测动态响应变迟钝。Q和R的比值才是决定滤波器行为的关键。Q/R大滤波器更信任观测响应快但噪声大Q/R小更信任模型平滑但滞后。我一般把R按实测方差固定然后只调Q一个参数这样调试维度只有一维容易收敛。3.4 用NumPy做离线对拍验证在板子上调试卡尔曼的效率很低因为你没法实时看中间量。我的做法是先在PC上用NumPy把整套算法跑一遍用记录下来的真实数据离线验证确认模型和参数没问题了再移植到板子上。NumPy写卡尔曼特别短十几行的事import numpy as np x np.array([[0.0], [0.0]]) P np.eye(2) * 1.0 F np.array([[1.0, dt], [0.0, 1.0]]) H np.array([[0.0, 1.0]]) Q np.diag([q_angle, q_gyro]) R np.array([[r_gyro]]) for z in measurements: x F x P F P F.T Q S H P H.T R K P H.T np.linalg.inv(S) x x K (np.array([[z]]) - H x) P (np.eye(2) - K H) P对拍的关键是把你板子上记录的陀螺仪原始数据导出成csv用这段NumPy代码跑一遍再用板子的mcu代码跑一遍同样的数据可以通过串口回放比较两者的输出。如果对不上大概率是维度、单位或者乘法顺序的问题。这个流程能帮你把90%的移植bug拦在板子之外。我个人的经验先把NumPy版本的参数调到一个满意状态把当时所有矩阵的初始值原样抄到C代码里。别在两边分别调否则你永远不知道差异是来自代码还是参数。4. 数值实操中的坑与排查记录4.1 协方差矩阵失对称、失正定的修复手法浮点运算的舍入误差会让协方差矩阵P慢慢失去对称性。理论上来讲P永远对称但计算机会让你看到P[0][1]和P[1][0]差了一个很小的数。这个误差短期不要紧长期累积可能导致P失去正定性Cholesky分解失败然后输出NaN。修复方法很直接每次更新完P之后强制对称化P (P Pᵀ)/2。这一行操作的代价很小但能显著提升长期稳定性。进一步的正定性保证需要更工程化的做法。协方差更新除了标准形式P (I−KH)P⁻还有一个叫Joseph形式// Joseph form: P (I-KH)P(I-KH)^T K R K^TJoseph形式的计算量大一些但它在数值上保证结果对称且半正定即使K不是最优增益也不会出负定。在RM这种控制周期很短、跑很久不能重启的场景里用Joseph形式换来的稳定性是值得的。还有一个兜底手段每次更新后检查P的对角线元素如果出现负数就把它钳到一个小正数。这个操作有点粗暴但它能防止滤波器因为一次数值意外彻底崩溃。我一般不推荐依赖钳位但作为最后的保险可以留着。4.2 单位、量纲、坐标系不一致的连锁反应这是新手最容易踩、又最不容易发现的坑。卡尔曼滤波的公式本身是量纲无关的但你的代码是有量纲的。如果dt用的是毫秒、速度用的是米每秒、角度用的是度那一堆数字乘在一起你根本看不出错在哪因为量纲不会在编译期报错。我列几个具体的检查点。dt的单位必须是秒因为角速度的单位一般是弧度每秒。角度的单位统一用弧度别一会儿度一会儿弧度三角函数库基本都是吃弧度的。旋转矩阵的方向是机体系到世界系还是反过来这个搞反了滤波器会往完全错误的方向收敛而且因为数学成立你不会收到任何报错。坐标系这块再展开说一下。RM的IMU装在云台上陀螺仪测的是机体系角速度但姿态通常定义在世界系。从机体系到世界系的转换用旋转矩阵R_wb。卡尔曼的F矩阵如果涉及坐标系转换必须把R_wb乘进去。很多同学直接在机体系里做积分然后奇怪为什么云台转一圈角度不对——因为你积分的是机体系角速度而机体系本身在转。排查这类问题最快的办法是做单轴旋转实验只让云台绕yaw轴慢慢转看估计角度和实际角度对不对得上。对不上再单独测pitch、roll。三个轴逐个验证过坐标系的问题基本就暴露了。4.3 常见问题速查表现象可能原因排查与解决状态估计发散到无穷Q过大或H设置让某状态不可观测检查H秩减小Q确认状态可观测性输出NaN协方差矩阵失去正定Cholesky失败强制对称化改用Joseph形式加钳位响应迟钝跟不上真实值R过大或Q过小重新标定R增大Q输出抖得厉害R过小或Q过大增大R减小Q新息长期偏一边模型有系统偏差或状态量缺失检查F、H模型考虑增加状态维度静止时角度缓慢漂移角速度零偏未建模把零偏加入状态向量一起估计三个轴互相干扰忽略了轴间耦合用完整协方差矩阵不要各轴独立跑这张表是我自己备赛期间记下来的基本上遇到的坑都能对上号。速查表的价值在于你在板子前抓耳挠腮的时候有个东西能快速帮你定位方向而不是从头推公式。一个独家心得调试卡尔曼的时候把P的对角线和卡尔曼增益K都打印出来。看P能判断滤波器自不自信看K能判断它更信观测还是更信模型。这两个量的变化趋势比状态输出本身更能告诉你滤波器是不是健康。5. 学习路径史荣昌那本书到底怎么用5.1 别一上来就啃证明史荣昌的《矩阵分析》和《矩阵分析引论》是国内很多学校的指定教材内容全、体系严谨但它是为数学系和研究生课程写的证明密度很高。如果你拿它当第一本矩阵入门书从头读到尾大概率读到第三章就放弃了。我的用法是把它当工具书。你需要什么查什么要用到矩阵求导了查矩阵微分那一章要用到特征值分解了查矩阵分解那一章。查的时候重点看结论和使用条件证明能看懂就看看不懂先跳过等用顺手了再回头补。配合着看可以再找一本偏工程的线性代数教材打底把向量、矩阵、线性变换的几何直观建立起来然后再回来看史荣昌的书会顺很多。顺序反了的话你会觉得什么都懂一点但什么都用不上。5.2 三周入门练习清单如果要给一个可执行的计划我会这么排。第一周把矩阵的加、乘、转置、逆、行列式、秩这些基础操作在纸上和NumPy里各过一遍重点是建立维度直觉看到公式就知道每个矩阵是几乘几。同时搞清楚什么是正定、什么是对称。第二周专项攻克协方差矩阵和矩阵求导。协方差矩阵要理解它在描述什么能自己写出一个二维协方差矩阵并解释每个元素。矩阵求导把前面说的两个核心公式和几个迹的恒等式过一遍然后找一份卡尔曼推导自己推一遍。第三周把卡尔曼的五个公式用NumPy实现跑通一个简单例子比如用模拟的带噪声数据估计一个匀速运动的位置和速度。跑通之后再把同样的模型换成RM里云台角度的场景用记录的数据验证。三周下来你对卡尔曼的理解会超过大多数只会调库的人。我个人体会最深的一点是矩阵分析这东西光看是没有用的必须动手写。你写代码的时候被维度对不上折磨几次看推导的时候卡住几回那些符号才真正变成你自己的东西。我们队里进步最快的队员都不是天赋最好的而是那种愿意把每个矩阵多敲一遍、把每个中间量多打印一次的。滤波器不会骗你你糊弄它它就用发散回敬你。
从矩阵分析到STM32：卡尔曼滤波工程落地指南
矩阵这个东西我见过太多RM电控新队员在第一次翻卡尔曼滤波推导的时候被劝退——满屏的加粗大写字母、上标转置、下标k还有那个看着就不想动的(PHᵀ(HPHᵀR)⁻¹)。但真正卡住人的从来不是卡尔曼滤波本身而是它默认你已经熟练的矩阵语言。我们中科大战队在带新人的时候有个不成文的规矩谁想碰卡尔曼先花两周把矩阵分析的基本操作过一遍能自己手推一遍增益公式再说。这篇就聊聊为什么卡尔曼滤波之前必须先啃矩阵矩阵分析里哪些东西是电控真正用得到的以及怎么把纸上的矩阵公式落到STM32的代码里还不炸。适合刚进RM电控组、听说过卡尔曼但没敢下手的同学也适合任何想把状态估计从抄库变成自己写的人。1. RM电控里卡尔曼滤波到底在算什么1.1 陀螺仪漂移和加速度计噪声是一对天生的冤家先说清楚我们为什么要用卡尔曼。RM的云台和底盘上姿态估计几乎都绕不开IMU。陀螺仪的好处是短时间响应快、动态准坏处是积分之后会有零偏累积你放那儿不动一分钟之后角度能飘出去好几度。加速度计正好相反静态的时候它能给你一个绝对的重力方向参考但机器人一跑起来、电机一抖、底盘一颠加速度计输出的就不只是重力了高频噪声大得没法直接用。这两者一个信短期一个信长期单独用都不行。卡尔曼滤波干的事情本质就是给这两个传感器的输出各自配一个信任度然后按最优的方式加权融合。这个信任度在数学上就是方差和协方差写成矩阵就是协方差矩阵。你如果只学过一维的加权平均脑子里想的是陀螺仪权重0.98加速度计权重0.02这在一维场景下勉强能跑。但真实的姿态估计里状态往往不只一个角度还有角速度、零偏观测也不只一路这时候权重就从一个数变成了一整个矩阵也就是卡尔曼增益K。所以第一层认知要先立起来卡尔曼滤波不是两路数据加权平均这么简单它是多维状态在多维观测下的最优估计。而多维这两个字就注定了你必须用矩阵来描述状态、描述不确定性、描述观测关系。1.2 标量思维的天花板在哪里我见过有同学写了个一维卡尔曼用来融合单个轴的陀螺仪和加速度计跑得还挺顺然后他试图把这个代码复制三份去处理roll、pitch、yaw三个轴。结果yaw轴死活收敛不了因为yaw没有绝对参考加速度计在水平旋转时给不出任何yaw信息三个轴之间存在耦合而他的代码里三个轴是独立跑的完全没考虑耦合。这就是标量思维的天花板。当你把三个轴独立处理的时候你隐含假设了三个轴的误差是互不相关的也就是协方差矩阵是个对角阵。实际情况里传感器误差、坐标系旋转、动力学耦合都会让这个矩阵出现非对角项。非对角项一出现标量那套加减乘除就彻底不够用了你必须上矩阵。还有一个更隐蔽的点坐标系变换。陀螺仪测的是机体系下的角速度但你要的是世界系下的姿态角。从机体系到世界系的转换就是一堆旋转矩阵相乘。旋转矩阵是矩阵分析里最基础的对象之一正交、行列式为1、转置等于逆这些性质你如果没在矩阵课上学过看姿态解算代码的时候会一头雾水——为什么这里要转置为什么那里要乘逆完全靠背。2. 卡尔曼滤波前必须打通的五个矩阵概念2.1 矩阵本质上是一个线性变换别只当成数字表格很多教材一上来就讲矩阵是数字排成的矩形阵列然后开始讲加减乘除。这种讲法对考试有用但对理解卡尔曼没用。你脑子里要建立的第一个直觉是矩阵是一个把向量从一个空间搬到另一个空间的变换。举个例子一个3×3的旋转矩阵R乘上一个表示机体角速度的三维向量得到的就是世界系下的角速度。这个矩阵不产生新信息它只是换了描述问题的坐标系。理解了这一点你就能明白为什么卡尔曼的预测方程里状态转移矩阵F乘的是状态向量x——它在描述上一时刻的状态经过系统的动力学变成了这一时刻的状态而这个变成的过程是线性的所以能用矩阵表示。矩阵作为线性变换还带来几个重要推论。可逆的矩阵对应一一对应的变换不可逆或者接近不可逆的矩阵对应信息丢失的变换。卡尔曼滤波里那个求逆操作(HPHᵀR)⁻¹本质就是在问这个观测能提供多少新信息如果这个矩阵接近奇异说明观测几乎提供不了新信息数值上就会抖。后面讲数值稳定性的时候会再回来说这一点。另外要习惯的符号约定一般用大写粗体或者大写字母表示矩阵如F、H、P小写粗体表示向量如x、z斜体小写表示标量。元素用双下标比如a_ij表示第i行第j列。这套约定不是装饰它能让你在读推导的时候一眼分清哪些是标量运算、哪些是矩阵运算。转置用上标ᵀ逆用上标⁻¹这两个符号在卡尔曼公式里出现的频率高到你必须形成肌肉记忆。2.2 矩阵乘法、转置与逆运算规则背后的物理含义矩阵乘法是卡尔曼滤波里用得最多的运算。规则你肯定知道A(m×n)乘B(n×p)得到C(m×p)C的元素是A的行点乘B的列。但我要强调的是维度匹配这个习惯。新手最常犯的错误就是矩阵维度对不上然后对着报错发呆。卡尔曼的预测方程 P⁻ FPFᵀ Q你数一下维度如果状态是n维F是n×nP是n×nFᵀ也是n×nQ是n×n。全对上了这个方程的物理含义是不确定性的传播——系统演化会把原来的不确定性F和P各拉伸一次再加上过程噪声Q带来的新增不确定性。矩阵乘法不满足交换律AB不等于BA这一点在推导卡尔曼的时候非常关键。为什么更新方程里是K(z-Hx̂)而不是(z-Hx̂)K为什么先算K再算x̂顺序不能乱。有同学写代码的时候把乘法顺序搞反了编译器不报错因为维度刚好凑得上但结果就是错的。我的建议是写矩阵代码的时候每个乘法后面用注释标出维度比如// F(n×n) * x(n×1) (n×1)这个习惯能帮你省下大量调试时间。转置的物理含义在协方差矩阵上体现得最明显。因为协方差矩阵是对称的P等于Pᵀ所以你看到FPFᵀ这个形式就可以理解为把F方向的变换同时施加在协方差矩阵的两侧保证结果的对称性。如果只写FPF结果就不对称了那它就不再是一个合法的协方差矩阵。逆矩阵是卡尔曼里的重操作。理论上你会写(A)⁻¹但代码里几乎从来不会真的去求逆。原因有两个一是求逆数值不稳定尤其是矩阵接近奇异的时候二是求逆的复杂度高在单片机上用不起。实际做法是把式子K P⁻Hᵀ(HP⁻HᵀR)⁻¹改写成解线性方程组的形式。设S HP⁻HᵀR那么看K P⁻HᵀS⁻¹等价于解SᵀKᵀ (P⁻Hᵀ)ᵀ或者更常见的是用Cholesky分解把S拆成LLᵀ然后前代回代求解。这就是后面代码实现里的重点。2.3 行列式、秩与可逆性决定滤波器崩不崩行列式在卡尔曼代码里你几乎不会直接算但它是理解为什么矩阵会病态的关键。一个方阵的行列式为0意味着它不可逆也意味着它对应的线性变换把空间压扁了——比如把一个三维空间压成一个平面。在卡尔曼滤波里如果观测矩阵H把不同状态压到了同一个观测上比如两个状态都只体现在同一个观测量里那你的观测就提供不了足够信息去区分这两个状态滤波器就会发散。秩刻画的是矩阵有效维度。一个n×n矩阵如果秩小于n就说明它的列向量线性相关变换之后有信息丢失。落到卡尔曼上状态转移矩阵F如果降秩意味着系统的某些状态会消失那预测就没意义了。观测矩阵H如果降秩意味着观测不足以支撑状态估计。这里有个实操经验拿到一个滤波发散的问题先去看H矩阵和F矩阵的秩再去调Q和R。我见过太多人一遇到发散就疯狂调Q调了半天治标不治本问题其实出在模型本身——H矩阵设置得让某个状态永远观测不到那这个状态的不确定性就会无限增长怎么调参都救不回来。举个RM里常见的例子。你做云台yaw轴估计状态设成[角度, 角速度]观测只有陀螺仪的角速度那么H [0, 1]。这时候角度这个状态完全没有观测。如果你不做任何处理纯靠F积分角度会很快飘走协方差矩阵P的角度对应项会线性增长到很大。这不是滤波器的bug而是你没有提供角度的绝对参考这个物理事实在矩阵上的体现。2.4 协方差矩阵与正定性不确定性怎么用矩阵描述协方差矩阵是卡尔曼滤波的灵魂但很多人对它的理解只停留在对角线是方差这一层。对角线元素确实是各个状态的方差但非对角元素才是关键它描述的是状态之间的相关性。比如你的速度估计偏高往往角度估计也会偏高这种相关性就体现在协方差矩阵的非对角项上。协方差矩阵有两个必须记住的数学性质对称以及半正定严格说在满秩情况下是正定。正定的定义是对于任何非零向量x都有xᵀPx 0。这个性质不是数学家闲着没事定的它有实实在在的物理意义——不确定性在任何方向上的投影都必须是正的不能有负的方差。一旦你的代码算出了一个非正定的P说明数值出问题了滤波器马上就要崩。正定性在卡尔曼里还和Cholesky分解直接挂钩。Cholesky分解要求矩阵正定你把S HPHᵀR用Cholesky拆成LLᵀ的前提就是S正定。而S是不是正定理论上H、P、R都是正定或者半正定的加起来的S是正定的所以能用Cholesky。但如果数值误差让P失去了正定性Cholesky就会失败或者出NaN。这就是为什么很多卡尔曼实现里要有重正定化的兜底操作。那么协方差矩阵在实际标定时怎么理解P的初始值大表示我一开始对状态一无所知P的初始值小表示我一开始就挺确定。P会随着预测变大不确定性增加随着更新变小观测带来了信息。你看P的大小就能判断滤波器当前自信不自信。调试的时候把P的对角线打印出来比看状态估计值有用得多。2.5 矩阵求导是卡尔曼增益公式的真正来源前面讲的都是会用这一节讲为什么对。卡尔曼滤波的更新公式x̂ x̂⁻ K(z-Hx̂⁻)里的K不是拍脑袋定的它是最小化估计误差协方差推导出来的最优点。推导过程要用到矩阵求导这是很多人跳过的一步但跳过之后你对整个滤波器的理解就浮在表面。核心要掌握的求导公式不多最常用的两个一是标量对向量求导∂(aᵀx)/∂x a二是二次型对向量求导当A对称时∂(xᵀAx)/∂x 2Ax。卡尔曼增益的推导里要把后验误差协方差的迹trace对K求导并令其为零过程中会反复用到二次型的导数公式。还有一个工具是矩阵微分恒等式尤其是涉及迹的那些∂tr(AB)/∂A Bᵀ这类。你不需要背全部但要能查得到、看得懂。我的建议是第一次推导的时候照着教材慢慢推一遍把每一步用到哪个公式标注在旁边。推完一遍之后你会发现卡尔曼增益那个看着吓人的(PHᵀ(HPHᵀR)⁻¹)其实就是一个观测信息与先验信息之比的矩阵版本。史荣昌那本《矩阵分析》和《矩阵分析引论》里求导相关的章节偏理论符号约定也偏数学。看得头晕是正常的。我的经验是把它当成工具书——需要哪个恒等式去查哪一节不要试图一次性从头读到尾。真正让你理解卡尔曼求导的是你在纸上跟着推一遍而不是读十遍别人怎么推。3. 把矩阵公式落到嵌入式代码里3.1 五个滤波公式的矩阵对照先把卡尔曼的五个公式用矩阵形式列清楚这是后面写代码的蓝图。状态向量x是n×1观测向量z是m×1状态转移矩阵F是n×n观测矩阵H是m×n过程噪声协方差Q是n×n观测噪声协方差R是m×m协方差矩阵P是n×n卡尔曼增益K是n×m。步骤公式维度状态预测x̂⁻ F x̂ B un×1协方差预测P⁻ F P Fᵀ Qn×n计算增益K P⁻Hᵀ(HP⁻HᵀR)⁻¹n×m状态更新x̂ x̂⁻ K(z − H x̂⁻)n×1协方差更新P (I − KH)P⁻n×n维度对不上的地方一眼就能看出来。写代码的时候我强烈建议把这五步分别封成函数不要让它们揉在一个大函数里。原因很实际调参和调试的时候你要单独打印某一步的中间量分开了才好插断点和打印。状态更新里那个(z − Hx̂⁻)叫新息也就是观测值和预测的观测值之间的差。这个量的物理含义是预测错在哪。如果新息一直很大说明你的模型F、H有问题如果新息抖动得厉害说明R设小了如果新息长期偏一边说明状态量或者模型有系统性偏差。调试卡尔曼盯着新息比盯着状态输出更有用。3.2 单片机上的小矩阵库怎么写才不出错RM的板子基本都是STM32跑裸机或者RTOS没有numpy这种库。你需要一个轻量的矩阵库。有人会去搬DSP库或者其他开源库但对于入门阶段我建议自己写一遍因为写的过程就是理解维度的过程。关键设计原则有几条。第一通信用静态数组不用动态内存。n和m在编译期就确定比如状态6维、观测3维用float A[6][6]这种形式避免malloc带来的碎片和不确定性。第二函数只处理确定的维度别为了通用性写成带循环变量维度的形式单片机上的通用性不值那点性能牺牲。第三对单位矩阵、转置、矩阵乘这几个高频操作单独优化。下面给一个六维状态下的核心操作示例用C写#define N 6 typedef float mat_t[N][N]; void mat_mul(const mat_t A, const mat_t B, mat_t C) { for (int i 0; i N; i) for (int j 0; j N; j) { C[i][j] 0.0f; for (int k 0; k N; k) C[i][j] A[i][k] * B[k][j]; } }这个三层循环是标准写法但在单片机上有优化空间。比如循环顺序可以改成i-k-j让内层循环连续访问内存缓存命中率更高。不过对于6维这种小矩阵优化的收益不大保持代码清晰更重要。等你的状态维度大到十几维、控制周期又很短的时候再考虑用DSP指令或者查表优化。求逆那块不要用高斯-约当消元去求通用逆用Cholesky分解解方程更稳。Cholesky的代码也不长三十行左右网上有成熟参考。关键是它在S正定的时候过程稳定而且分解完一次可以复用来做前代回代。3.3 F、H、Q、R四个矩阵的构造实操与参数选择模型矩阵的构造决定了滤波器能不能用。以RM云台姿态估计里最常见的角度角速度模型为例状态x [θ, ω]ᵀ观测是陀螺仪角速度z ω_gyro。状态转移矩阵F按匀速模型近似θ(k) θ(k−1) ω(k−1)·dtω(k) ω(k−1)所以// dt 单位为秒 F[0][0] 1.0f; F[0][1] dt; F[1][0] 0.0f; F[1][1] 1.0f;观测矩阵H其实只用观测角速度H [0, 1]。过程噪声Q反映模型的不可信程度。匀速模型忽略了角加速度所以角度方程的不确定性来自角加速度。工程上常用的Q构造方式是取一个加速度噪声参数q角度项为q·dt³/3速度项为q·dt交叉项为q·dt²/2。你可以先不纠结这个公式怎么来的先用经验值调看收敛效果。观测噪声R就是陀螺仪读数的方差。这个可以在静止状态下采几百个点直接算方差是个很实在的初始化方法比瞎猜好得多。// 静止采样计算 R float mean 0, var 0; for (int i 0; i 500; i) mean data[i]; mean / 500; for (int i 0; i 500; i) var (data[i]-mean)*(data[i]-mean); var / 500; R[0][0] var;注意R的标定一定要在机器人静止、电机不转、没有外力干扰的条件下做。如果标定的时候云台在抖动算出来的方差会偏大滤波器就会过度信任模型、不够信任观测动态响应变迟钝。Q和R的比值才是决定滤波器行为的关键。Q/R大滤波器更信任观测响应快但噪声大Q/R小更信任模型平滑但滞后。我一般把R按实测方差固定然后只调Q一个参数这样调试维度只有一维容易收敛。3.4 用NumPy做离线对拍验证在板子上调试卡尔曼的效率很低因为你没法实时看中间量。我的做法是先在PC上用NumPy把整套算法跑一遍用记录下来的真实数据离线验证确认模型和参数没问题了再移植到板子上。NumPy写卡尔曼特别短十几行的事import numpy as np x np.array([[0.0], [0.0]]) P np.eye(2) * 1.0 F np.array([[1.0, dt], [0.0, 1.0]]) H np.array([[0.0, 1.0]]) Q np.diag([q_angle, q_gyro]) R np.array([[r_gyro]]) for z in measurements: x F x P F P F.T Q S H P H.T R K P H.T np.linalg.inv(S) x x K (np.array([[z]]) - H x) P (np.eye(2) - K H) P对拍的关键是把你板子上记录的陀螺仪原始数据导出成csv用这段NumPy代码跑一遍再用板子的mcu代码跑一遍同样的数据可以通过串口回放比较两者的输出。如果对不上大概率是维度、单位或者乘法顺序的问题。这个流程能帮你把90%的移植bug拦在板子之外。我个人的经验先把NumPy版本的参数调到一个满意状态把当时所有矩阵的初始值原样抄到C代码里。别在两边分别调否则你永远不知道差异是来自代码还是参数。4. 数值实操中的坑与排查记录4.1 协方差矩阵失对称、失正定的修复手法浮点运算的舍入误差会让协方差矩阵P慢慢失去对称性。理论上来讲P永远对称但计算机会让你看到P[0][1]和P[1][0]差了一个很小的数。这个误差短期不要紧长期累积可能导致P失去正定性Cholesky分解失败然后输出NaN。修复方法很直接每次更新完P之后强制对称化P (P Pᵀ)/2。这一行操作的代价很小但能显著提升长期稳定性。进一步的正定性保证需要更工程化的做法。协方差更新除了标准形式P (I−KH)P⁻还有一个叫Joseph形式// Joseph form: P (I-KH)P(I-KH)^T K R K^TJoseph形式的计算量大一些但它在数值上保证结果对称且半正定即使K不是最优增益也不会出负定。在RM这种控制周期很短、跑很久不能重启的场景里用Joseph形式换来的稳定性是值得的。还有一个兜底手段每次更新后检查P的对角线元素如果出现负数就把它钳到一个小正数。这个操作有点粗暴但它能防止滤波器因为一次数值意外彻底崩溃。我一般不推荐依赖钳位但作为最后的保险可以留着。4.2 单位、量纲、坐标系不一致的连锁反应这是新手最容易踩、又最不容易发现的坑。卡尔曼滤波的公式本身是量纲无关的但你的代码是有量纲的。如果dt用的是毫秒、速度用的是米每秒、角度用的是度那一堆数字乘在一起你根本看不出错在哪因为量纲不会在编译期报错。我列几个具体的检查点。dt的单位必须是秒因为角速度的单位一般是弧度每秒。角度的单位统一用弧度别一会儿度一会儿弧度三角函数库基本都是吃弧度的。旋转矩阵的方向是机体系到世界系还是反过来这个搞反了滤波器会往完全错误的方向收敛而且因为数学成立你不会收到任何报错。坐标系这块再展开说一下。RM的IMU装在云台上陀螺仪测的是机体系角速度但姿态通常定义在世界系。从机体系到世界系的转换用旋转矩阵R_wb。卡尔曼的F矩阵如果涉及坐标系转换必须把R_wb乘进去。很多同学直接在机体系里做积分然后奇怪为什么云台转一圈角度不对——因为你积分的是机体系角速度而机体系本身在转。排查这类问题最快的办法是做单轴旋转实验只让云台绕yaw轴慢慢转看估计角度和实际角度对不对得上。对不上再单独测pitch、roll。三个轴逐个验证过坐标系的问题基本就暴露了。4.3 常见问题速查表现象可能原因排查与解决状态估计发散到无穷Q过大或H设置让某状态不可观测检查H秩减小Q确认状态可观测性输出NaN协方差矩阵失去正定Cholesky失败强制对称化改用Joseph形式加钳位响应迟钝跟不上真实值R过大或Q过小重新标定R增大Q输出抖得厉害R过小或Q过大增大R减小Q新息长期偏一边模型有系统偏差或状态量缺失检查F、H模型考虑增加状态维度静止时角度缓慢漂移角速度零偏未建模把零偏加入状态向量一起估计三个轴互相干扰忽略了轴间耦合用完整协方差矩阵不要各轴独立跑这张表是我自己备赛期间记下来的基本上遇到的坑都能对上号。速查表的价值在于你在板子前抓耳挠腮的时候有个东西能快速帮你定位方向而不是从头推公式。一个独家心得调试卡尔曼的时候把P的对角线和卡尔曼增益K都打印出来。看P能判断滤波器自不自信看K能判断它更信观测还是更信模型。这两个量的变化趋势比状态输出本身更能告诉你滤波器是不是健康。5. 学习路径史荣昌那本书到底怎么用5.1 别一上来就啃证明史荣昌的《矩阵分析》和《矩阵分析引论》是国内很多学校的指定教材内容全、体系严谨但它是为数学系和研究生课程写的证明密度很高。如果你拿它当第一本矩阵入门书从头读到尾大概率读到第三章就放弃了。我的用法是把它当工具书。你需要什么查什么要用到矩阵求导了查矩阵微分那一章要用到特征值分解了查矩阵分解那一章。查的时候重点看结论和使用条件证明能看懂就看看不懂先跳过等用顺手了再回头补。配合着看可以再找一本偏工程的线性代数教材打底把向量、矩阵、线性变换的几何直观建立起来然后再回来看史荣昌的书会顺很多。顺序反了的话你会觉得什么都懂一点但什么都用不上。5.2 三周入门练习清单如果要给一个可执行的计划我会这么排。第一周把矩阵的加、乘、转置、逆、行列式、秩这些基础操作在纸上和NumPy里各过一遍重点是建立维度直觉看到公式就知道每个矩阵是几乘几。同时搞清楚什么是正定、什么是对称。第二周专项攻克协方差矩阵和矩阵求导。协方差矩阵要理解它在描述什么能自己写出一个二维协方差矩阵并解释每个元素。矩阵求导把前面说的两个核心公式和几个迹的恒等式过一遍然后找一份卡尔曼推导自己推一遍。第三周把卡尔曼的五个公式用NumPy实现跑通一个简单例子比如用模拟的带噪声数据估计一个匀速运动的位置和速度。跑通之后再把同样的模型换成RM里云台角度的场景用记录的数据验证。三周下来你对卡尔曼的理解会超过大多数只会调库的人。我个人体会最深的一点是矩阵分析这东西光看是没有用的必须动手写。你写代码的时候被维度对不上折磨几次看推导的时候卡住几回那些符号才真正变成你自己的东西。我们队里进步最快的队员都不是天赋最好的而是那种愿意把每个矩阵多敲一遍、把每个中间量多打印一次的。滤波器不会骗你你糊弄它它就用发散回敬你。