卡尔曼滤波中的矩阵分析：从状态向量到Eigen实战

卡尔曼滤波这四个字在RM电控群里每年都要被问上几百遍而大多数人的学习瓶颈与其说在滤波本身不如说在它前面那片“矩阵分析”的洼地。我见过太多队员卡在同一个地方一维卡尔曼的代码跑通了云台角度也能稳住但一换成二维、三维状态P矩阵、增益K、状态转移矩阵这些符号全部搅在一起代码抄过来改个维度编译倒是过了滤波输出却直接飞掉。这篇是【中科大RM电控合集】的前瞻篇专门解决这个问题。文章会从电控实战的角度把卡尔曼滤波真正要用到的矩阵分析知识拆开讲一遍——不追求数学上的严谨完备只追求一件事让你看完之后能自己写出状态向量的维度能对着运动学公式把A、B、H矩阵填对能理解协方差矩阵P的每个元素在说啥能把增益公式里的求逆运算稳稳落到Eigen代码里。适合刚接手电控、准备做云台稳定或者自瞄预测的同学也适合那些公式能默写、但一写矩阵就懵的“理论会了、手不会”选手。1. 为什么调车调了半年最后卡在了一堆矩阵符号上1.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 (1 - K H) P这套公式里全是标量。x是一个数P是一个数K也是一个数。理解起来很直白x是你要估计的物理量P是估计的可信度K是根据传感器噪声算出来的“修正比例”。把这个过程想明白一维卡尔曼就算入门了。但回到RM赛场你面对的是什么云台yaw轴的角度和角速度、自瞄里敌方装甲板在图像上的x、y像素坐标以及它的移动速度、或者小陀螺模式下目标位置随时间的变化。哪怕是最朴素的匀速运动模型状态也有两个分量位置和速度。一维公式里的每个标量到这里都得变成矩阵或向量。这一变很多人就乱了。乱在哪我观察到的核心问题是卡尔曼滤波从来不是“一维算法的多个副本”而是这些分量之间存在耦合。比如云台角度跟角速度天然耦合——角度是角速度的积分。如果你把两个维度拆开各滤各的等于告诉滤波器“角度变化跟角速度无关”这个模型从根本上就是错的。耦合关系由状态转移矩阵A来表达矩阵分析正好就是处理这种“多个变量一起演化、互相影响”的数学工具。1.2 卡尔曼滤波真正用到的矩阵知识清单我梳理了一下实际写卡尔曼滤波代码矩阵分析的知识点用在这五个地方状态向量与观测向量的维度设计先确定要估计哪些量每个量就是一个维度。状态转移矩阵A、控制矩阵B、观测矩阵H的构建用运动学/动力学方程把系统模型写成矩阵形式。协方差矩阵P、过程噪声Q、测量噪声R的设置这些矩阵的维度、对称性、正定性直接决定滤波器能不能跑稳。矩阵乘法与矩阵求逆预测步的核心是A P A^T更新步的核心是求逆(H P H^T R)^(-1)。特征值与可观测性分析用来判断你的模型和传感器配置能不能让滤波器收敛。这五块对应到矩阵分析教材里基本就是向量空间、矩阵乘法、矩阵的逆、特征值这几章。所以我的观点一直很明确与其一上来死磕卡尔曼的完整推导不如先把矩阵分析的基础磨一遍。磨完之后再回头看书你会发现推导的每一步都落在这几个矩阵操作上公式就是“自然的下一步”根本不需要硬背。1.3 最常见的误区把标量公式塞进数组我有一个印象很深的经历。当年第一次把一维代码改成二维时想当然地把A、P、K都写成了数组每个分量独立套公式。结果角度很稳、速度却剧烈震荡怎么调Q和R都没用。后来才明白问题出在哪卡尔曼的每个矩阵乘法都在“混合”不同维度的信息。A P A^T里A的非对角元素会把角速度的协方差耦合到角度上这个耦合恰恰是滤波器的“常识”——知道角度在变就能推测角速度独立滤波把这个常识丢掉了速度估计自然没有修正来源只能靠测量残差硬拉拉出来的结果就是震荡。这个误区在RM群里几乎每周都有人踩所以我特意放在最前面说。避免它的方法只有一个把矩阵当成一个整体严格按照矩阵乘法规则来写而不是“对每个分量循环”。写代码时遇到矩阵乘法先停下来用纸笔把每个矩阵的维度标清楚再动手写。1.4 这篇前瞻篇该怎么用如果你现在刚接触卡尔曼建议按顺序读第2章先把状态空间模型的概念建起来第3章理解协方差矩阵第4章看懂增益计算第5章是判断滤波器能不能收敛的理论工具第6章是完整的代码落地参考第7章是教材推荐和避坑清单。如果你已经写过一版二维卡尔曼可以直接跳到第3章和第4章看P矩阵和求逆的部分再对照第6章的代码检查自己的实现。如果你是在调参阶段卡住了重点看第7章的常见错误表那六类问题基本覆盖了RM电控里90%的卡尔曼“玄学”现象。2. 状态空间模型用向量和矩阵描述你的机器人2.1 先定状态向量把要估计的量写成一个向量在写任何卡尔曼滤波之前第一件事是回答“我想估计哪些量”这些量拼在一起就是状态向量x。以云台yaw轴为例状态通常是x [θ, ω]^Tθ是角度ω是角速度。之所以把角速度放进来是因为陀螺仪和编码器直接测的是角度但角速度的变化趋势可以帮助预测下一个时刻的角度。尤其在视觉自瞄场景里目标可能被遮挡几帧这段时间内滤波器只能靠模型预测状态里有没有角速度或者目标速度预测的准确性差别非常大。状态向量的选择直接影响后续所有矩阵的尺寸。基本原则是能不多选就不多选。每多一个状态分量P矩阵就多一行一列计算量按平方涨。虽然现在主控芯片算力普遍够用但调试周期会成倍拉长——你得给每个状态分量设置合理的噪声初值还得一个一个验证它对滤波结果的贡献。我见过有人给云台滤波一上来就设计了六维状态结果调了一个月都没调稳把状态砍到三维之后半天就收敛了。2.2 状态转移矩阵A上一时刻如何变成下一时刻系统从k-1时刻到k时刻的变化用状态转移矩阵A描述x_k A x_{k-1} B u_k w_k这个式子的含义是下一时刻的状态等于当前状态按照某种规律演化再加上外部输入的影响和随机扰动。假设匀速模型时间间隔为dtθ_k θ_{k-1} ω_{k-1} * dt ω_k ω_{k-1}写成矩阵就是[θ_k] [1 dt] [θ_{k-1}] [ω_k] [0 1] [ω_{k-1}]所以A [[1, dt], [0, 1]]。这里dt很关键。如果视觉帧率不稳定dt每次都不一样A矩阵就不是常数每次predict都要重新填一次如果陀螺仪数据是固定频率A可以提前算好省掉一部分重复计算。如果希望模型更精细把角加速度α也作为状态x [θ, ω, α]^TA就变成3×3[1 dt 0.5*dt^2] [0 1 dt ] [0 0 1 ]这是匀加速模型的标准形式很多做弹道补偿的队伍会用这个模型。拿这个矩阵乘上状态向量你就能直观看到“预测”在做什么它只是用物理规律把当前状态外推到下一个时刻。这里没有魔法矩阵乘法就是把这几个运动学公式整整齐齐地算一遍。2.3 控制矩阵B与观测矩阵H输入和传感器各自的角色如果系统里有外部输入比如云台电机的目标速度指令或者自瞄的云台角速度前馈就用控制矩阵B来描述输入的贡献。B的维度是n×mn是状态维度m是控制输入维度。继续用yaw轴举例。假设电调接收的是角速度指令u模型写成θ_k θ_{k-1} (ω_{k-1} u_k) * dt ω_k ω_{k-1}那B [dt, 1]^Tu是电机角速度指令。放在真实场景里这意味着云台转向的时候滤波器的预测不再只依赖上一时刻的角速度还会考虑电机当前正在执行的动作预测误差会明显减少。观测矩阵H负责把状态映射到传感器读数。陀螺仪或者编码器直接读角度那H [1, 0]如果还有一路角速度测量值观测向量z [θ_meas, ω_meas]^TH就是单位阵[[1, 0], [0, 1]]。很多初学者搞不懂H的实质其实它就是在说“你的传感器能看见状态的哪几个分量”。看不见的分量H里对应位置是0就行。在RM场景里H通常很简单——角速度陀螺仪能看到那就留一个1某些状态分量没有直接传感器H里就是0滤波会通过模型耦合把它“估”出来。3. 协方差矩阵不确定性是如何被数学化的3.1 从方差到协方差矩阵卡尔曼滤波的核心不只是估计状态还要知道“这个估计有多可信”。这个可信度用协方差矩阵P表示。先回忆一维情况对单个随机变量x它围绕均值的平均波动用方差σ²衡量。方差越大说明这个估计越不可信。多维情况下每个分量有自己的方差而且分量之间还可能相关——比如角度估计偏差大时角速度估计往往也偏差大。这种相关性用协方差表示。所有方差和协方差拼在一起就是协方差矩阵P。对二维状态x [θ, ω]P [Var(θ) Cov(θ, ω)] [Cov(ω, θ) Var(ω) ]因为Cov(θ, ω) Cov(ω, θ)P一定是对称矩阵。这一点很多人体会不深但实际调代码时会遇到长期迭代加上浮点误差P矩阵会慢慢变得不对称甚至出现负的方差。这时候滤波器表面上还能跑实际已经处于数值崩溃的边缘了。一个常用的补救手段是每次更新后做一次对称化P (P P^T) / 2。3.2 A P A^T 那一步到底在干什么预测步的协方差更新长这样P_k A P_{k-1} A^T Q为什么是A P A^T而不是A²P这个问题我当年也问过。从线性代数的角度解释矩阵P定义了一个不确定区域二维时是椭圆三维以上是超椭球状态转移矩阵A对这个区域做的是线性变换——旋转加缩放。二次型A P A^T是把A的作用以“变换协方差”的正确方式写出来直接乘以A²会丢失旋转信息算出来的椭圆形状就错了。用一个直观例子说明如果P是单位阵表示角度和角速度的不确定性是独立的、大小相同的圆形不确定区。A [[1, dt], [0, 1]]对这个圆做剪切变换结果变成一个倾斜的椭圆——角度不确定性被dt放大θ和ω之间也出现了相关性。A P A^T算出来的就是这个剪切后椭圆的精确参数。这一步的意义在于预测不仅改变了状态估计值也改变了不确定性的大小和方向。滤波器要是把这个算错了后面的增益计算全是错的。3.3 Q和R的物理意义与调参起点Q表示你对模型的信任程度。模型越粗糙、扰动越大Q就越大。R表示对传感器的信任程度传感器噪声越大R就越大。它们的数值设定直接决定卡尔曼增益K的走向如果Q相对R很大滤波器更信任测量响应快但噪声大反之更信任预测平滑但滞后。在RM场景里Q和R往往是调参的焦点。我的经验是R可以用传感器数据统计出来。把车架起来让云台静止采集几百组陀螺仪读数算一下方差这个值就接近R的量级。Q则更多靠经验整定通常先设一个比较小的对角矩阵观察滞后和噪声的平衡再慢慢调整。一个常见的起步值Q [[0.001, 0], [0, 0.001]] R [[0.01]]实际调的时候你会发现Q太小会让卡尔曼增益偏小云台响应迟钝目标动了它追不上Q太大会让输出几乎不滤波跟原始测量一样抖。可以用二分法来回试一次改一个数量级比瞎猜效率高得多。4. 矩阵求逆与卡尔曼增益滤波器最核心的计算4.1 增益公式为什么必须求逆更新步的核心是卡尔曼增益K P H^T (H P H^T R)^(-1)括号里的H P H^T R表示“预测得到的测量不确定性”也就是传感器读数的不确定度。要求增益必须把这个矩阵求逆。矩阵求逆的数值性质直接决定滤波器的稳定性。如果这个矩阵接近奇异行列式接近0求逆结果会极其不稳定表现出来就是滤波输出出现尖峰甚至发散。在RM实车上这种情况并不罕见尤其是R设置过小、或者P矩阵经过长期迭代产生数值漂移时。所以增益计算这一行往往是整个滤波器里最值得关注的地方。4.2 两种极端情况帮你建立直觉第一种测量噪声R非常大。这时H P H^T R ≈ RK ≈ P H^T / RK变得很小滤波器基本不更新输出主要来自预测。对应实际场景是传感器数据野值、通信丢包时我们希望信任模型而不是测量。第二种测量噪声R很小。这时K趋近于一个让估计完全跟随测量的值相当于“直接相信传感器”。对应场景是裁判系统数据更新频率很高、噪声很低时几乎可以放弃预测直接拿测量当估计。增益矩阵K的维度是n×m状态维乘观测维。在代码里K不是一个“数”而是一个矩阵它决定每个状态分量应该以多大比例吸收测量残差。这也是为什么二维状态配合单观测时K是一个二维向量角度和角速度各自有独立的修正比例。4.3 嵌入式平台上的稳定求逆做法在嵌入式平台上直接调用Eigen的inverse()当然能用但有些经验值得分享。优先使用ldlt()或llt()分解求解线性方程组而不是显式求逆。H P H^T R是对称正定矩阵前提是R正定用Cholesky分解不但更快数值也更稳。在R的对角线上加一个极小的量比如1e-6防止矩阵奇异。定期检查P矩阵的对称性和正定性。如果发现对角元素出现负值说明数值已经坏了需要重置或做对称化处理。如果目标平台没有FPU比如老款Cortex-M4跑单精度运算矩阵运算尽量用float而不是double。运算量能省一半代价是需要偶尔检查数值漂移。这些细节在电脑仿真时几乎感受不到差异但放到实车上是能明显区分稳定性和调试效率的。5. 特征值与可观测性为什么有的滤波器就是调不收敛5.1 特征值告诉你模型本身的脾气分析一个线性系统A的性质最有力的工具是特征值和特征向量。A的特征值决定了系统随时间演化的模式实部为负的特征值对应衰减模式实部为正对应发散模式。对卡尔曼滤波来说A的特征值决定了预测模型的稳定性也影响滤波器的收敛速度。在RM电控里云台的匀速/匀加速模型的A特征值通常是1因为系统是纯积分型的。这说明模型本身不衰减——状态会一直保持不发散也不自动收敛。这其实不是坏事因为卡尔曼滤波的收敛性来自观测更新而不是模型本身。特征值的真正价值在于当你怀疑“滤波器怎么老是不收敛”时先算一下A的特征值和可观测性矩阵的秩能排除一大片模型设计层面的问题。5.2 可观测性矩阵传感器到底能不能看见全部状态卡尔曼滤波要正常工作系统必须满足可观测性条件——即通过一段时间的传感器数据能唯一确定所有状态分量。判断方法是构造可观测性矩阵O [H; H A; H A^2; ...; H A^(n-1)]如果O满秩系统可观测。判断满秩就是矩阵分析里“秩”这一章的内容。一个经典例子状态是[位置, 速度]传感器只能测位置H [1, 0]。对匀速模型A [[1, dt], [0, 1]]可观测性矩阵O [[1, 0], [1, dt]]。它的秩是2满秩说明速度是可观测的——这就是为什么卡尔曼滤波可以用位置测量间接得到速度估计。反过来如果A的设计让O降秩比如A是单位阵位置和速度之间没有任何耦合那速度就永远无法从位置观测中恢复。表现出来就是位置跟得挺好速度估计却一直漂。很多调参调不出来的情况根源就在这里——不是参数问题是模型结构问题。5.3 工程上的判断顺序假设你觉得滤波器的速度估计滞后太严重想让模型更快“忘记”旧状态可以考虑修改A的设计或者增大过程噪声Q。本质上你是在改变系统矩阵的谱性质这在矩阵分析里属于“矩阵扰动”和“特征值灵敏度”的范畴。但对电控同学来说不需要把理论全部嚼碎。记住一个判断顺序就够了滤波器输出长期不跟随真实值先查可观测性可观测性没问题再调Q、R的比值这两步都做完了还是有尖峰最后怀疑数值稳定性。这个顺序能省掉大量无效调参时间。我见过有人花了一周调Q、R最后发现是A矩阵某个元素赋值赋错了——如果先做可观测性和矩阵验证几分钟就能定位。6. 从公式到Eigen代码把矩阵运算落到电控板子上6.1 先用Eigen把矩阵搭起来在RM电控中Eigen是事实标准。它是纯头文件模板库几乎零依赖很方便嵌进STM32工程。最基本的写法#include Eigen/Dense using namespace Eigen; // 定义状态向量2维 Vector2d x; // [角度, 角速度] x 0.0, 0.0; // 定义状态转移矩阵 double dt 0.001; Matrix2d A; A 1.0, dt, 0.0, 1.0; // 协方差矩阵初始值 Matrix2d P; P 0.1, 0.0, 0.0, 0.1;注意Eigen默认列优先存储和C数组的行优先不同。日常用Eigen封装的运算符基本感受不到差异但如果需要把矩阵数据直接转成数组传给其他库就得搞清楚存储顺序了。6.2 一个完整的二维卡尔曼滤波类下面这个类实现了匀速模型的预测加更新直接可以用来做云台角度的滤波class KalmanFilter2D { public: KalmanFilter2D(double dt) : dt_(dt) { A_ 1.0, dt_, 0.0, 1.0; H_ 1.0, 0.0; // 只观测角度 Q_ 0.001, 0.0, 0.0, 0.001; R_ 0.01; x_ 0.0, 0.0; P_ 0.1, 0.0, 0.0, 0.1; } void predict() { x_ A_ * x_; P_ A_ * P_ * A_.transpose() Q_; } void update(double z) { // 残差 double y z - H_ * x_; // S H P H^T R double S (H_ * P_ * H_.transpose())(0, 0) R_(0, 0); // K P H^T / S Vector2d K P_ * H_.transpose() / S; // 更新状态与协方差 x_ x_ K * y; P_ P_ - K * H_ * P_; } private: double dt_; Matrix2d A_, P_, Q_; Matrixdouble, 1, 2 H_; Matrixdouble, 1, 1 R_; Vector2d x_; };这里有个细节S是标量因为观测维度是1。如果观测是两维比如同时测角度和角速度S就是2×2矩阵需要用ldlt().solve()而不是除法。6.3 嵌入式环境的性能细节固定尺寸优于动态尺寸。Matrixdouble, 2, 2是固定大小直接在栈上分配零动态内存MatrixXd的动态分配在嵌入式实时性敏感的场合不可取。开启编译优化加-O2或者-O3Eigen的表达式模板能自动把多个矩阵乘法合并优化。如果板子没有FPU尽量避免inverse()和复杂分解用float类型并且每一步都检查NaN。调试阶段把每一步的P、K、x打印到串口画成曲线看滤波效果比凭空想象参数往哪调有效得多。我在调试视觉引导云台时就是靠打印K值来判断滤波器有没有进入正常收敛状态。K如果长期接近0或者1多半是Q、R比例失衡先调参数再查代码方向就对了。7. 学习路线与避坑建议矩阵分析要怎么补才不白学7.1 教材怎么选史荣昌《矩阵分析第三版》中科大很多课程和实验室培训都推荐这本。内容覆盖面广线性空间、线性变换、矩阵分解、特征值、广义逆都有缺憾是部分推导比较简略适合有基础的人快速过。张贤达《矩阵分析与应用》更工程向大量应用实例信号处理、控制理论都有对照和卡尔曼滤波的衔接很好。如果要深入做状态估计这本书的参考价值很高。Linear Algebra Done RightSheldon Axler适合建立线性空间直觉但对工程计算帮助有限不建议作为唯一教材。我的建议是不要从头到尾读任何一本矩阵分析教材。以卡尔曼滤波为牵引按“向量空间到矩阵乘法到逆矩阵到特征值”的顺序只读相关章节然后立即用Eigen在模拟数据上跑一遍。理论跟实践交替推进效率比单纯啃书高得多。7.2 推荐动手路线我建议按下面的顺序走每一步都有明确产出第一步手写一维卡尔曼跑通位置估计。第二步把一维改成二维位置加速度理解A P A^T的耦合作用。第三步用Eigen实现二维卡尔曼对照手写结果。第四步加入H矩阵把单观测改成多观测。第五步给云台或陀螺仪数据加噪声测试Q、R变化对输出的影响。第六步回头看卡尔曼滤波的完整推导这时候你会发现每一步都能对应到矩阵分析的某个概念。第六步是关键。很多人一上来就死磕推导推导完还是不会写代码反过来先动手、再回头补理论理解深度完全不一样。7.3 调参现场最常见的六类错误错误表现原因处理方式维度不匹配编译报错或乘出NaN矩阵乘法顺序或维度弄错每次操作前打印矩阵维度检查A设成对角阵滤波效果跟独立滤波一样没有理解状态耦合用运动学模型推导AP初始化为全0滤波器不更新或收敛极慢初始信任度过高认为初始状态绝对准确给P对角线设合理初值如0.1R设置过小输出严重抖动过度信任测量用静止数据统计RQ设置过大输出几乎不滤波模型过于不可信逐步减小Q试调浮点数值漂移P不对称或出现负方差长期迭代误差积累定期对称化或重置P这六类问题我在调试中基本都踩全了。尤其是P初始化为全0那个坑看起来好像是在表达“我没有先验信息”实际上是在告诉滤波器“我百分百确定初始状态是0”。这样一来滤波器自然不敢用测量去更新表现出一股顽固的“我不信你”的态度。理解了这一点之后P的初始化我再也没有乱设过。最后说点题外话。我在实验室带新人的时候发现一个规律凡是卡尔曼用得好的队员没有一个是从完整推导开始学的。他们都是先拿一个能跑的例子把矩阵维度搞清楚把状态方程的物理意义想明白然后再回头补理论。矩阵分析不是门槛更像是一个工具箱——你不需要把每把工具的锻造过程都弄明白但至少要知道每一把是干什么的、怎么用、什么时候换一把。这篇文章的目的就是帮你把这个工具箱的第一层工具摆好。后续的合集里我会接着讲卡尔曼滤波的完整推导框架、扩展卡尔曼在自瞄里的实际应用以及赛场上那些“书上学不到”的工程细节。