卡尔曼滤波入门：概率统计、协方差与高斯分布基础全解析

打RoboMaster的同学应该都听过卡尔曼滤波尤其是做电控的老队员嘴里天天挂着“调Q、调R”可你要是追问一句“Q和R到底代表什么”很多人会突然卡壳。这不是因为笨而是因为一开始就跳过了最关键的数学地基直接去抄公式、调参数了。卡尔曼滤波说白了是一套在概率统计框架下做状态估计的算法它不管你是融合陀螺仪和视觉还是融合里程计和加速度计底层都在做同一件事用带噪声的观测去猜那个看不见的真值。所以这一篇“卡尔曼滤波前瞻”不碰滤波公式先把概率统计基础讲透适合电控组刚接触状态估计的队员也适合想系统补数学的老队员。我写这一篇的初衷是发现很多同学能把卡尔曼滤波的五个公式背下来但一提到“方差”“协方差”“先验后验”就开始含糊。问题就出在他们不知道每个符号在物理上到底对应什么。这篇内容会从传感器噪声讲起把随机变量、期望、方差、协方差、高斯分布、贝叶斯公式一个一个掰开最后再用一个最简单的一维位置估计器串联起来。你如果把这部分吃透后面再看卡尔曼滤波的推导会发现每一行公式都有了落地的意义。1. 为什么卡尔曼滤波绕不开概率统计1.1 传感器数据不是真值是一个分布先别急着碰卡尔曼滤波我问你一个很实际的问题当你从编码器里读到一个角度你真的认为这个角度就是当前的真实角度吗大概率不是。编码器有量化误差有装配误差有温度漂移陀螺仪有零偏有积分漂移摄像头测距更不用说光线、遮挡、算法方差全都会影响结果。在概率统计的视角下传感器输出不是一个确定的数而是一个随机变量的采样。所谓随机变量不是说这个值没有规律而是它的取值服从某种概率分布。比如某个测距模块的测量误差可能近似服从均值为0、标准差为2厘米的高斯分布。也就是说同一时刻你去读它读到的值会在真实距离附近波动但大多数情况下不会偏离太远。这个视角特别重要。因为一旦你承认传感器测量是带噪声的随机量你就会明白单次测量没有任何意义只有结合概率分布才能描述“它大概在什么位置、有多确定”。卡尔曼滤波做的事情本质上就是把多个带不确定性的测量按概率加权合并起来得到一个不确定性更小的估计。所以概率统计不是卡尔曼滤波的附庸而是它的底层语言。1.2 卡尔曼滤波在解决什么问题卡尔曼滤波解决的是一个很朴素的工程问题系统内部有一个你关心但无法直接测量的状态比如云台的绝对角度、小车的实时位置、弹道的飞行参数这些状态只能通过传感器间接观察。问题在于传感器观测既不是完全准确的状态本身也在随时间变化还受到外部扰动。你怎么在这么多不确定因素下尽量准确地估计出状态这就是状态估计问题。从概率角度看状态变量$x$是一个随机向量我们需要在每一时刻求出它的后验概率分布。卡尔曼滤波不是拍脑袋给个估计值而是把这个后验分布描述成高斯分布然后递归地计算它的均值和协方差矩阵。均值就是当前最优估计协方差矩阵就是对这个估计有多大的把握。所以卡尔曼滤波从来不只输出一个数它输出的是一个概率分布只是我们通常只取那个均值当结果用。理解这一点后你就会发现那些动不动说“卡尔曼滤波是用来去噪的”的说法其实是片面的。它确实能让曲线更平滑但它真正在做的是在预测模型和测量模型之间做最优权衡。这个“权衡”的尺度完全由概率统计中的方差和协方差决定。不理解协方差就不理解卡尔曼增益不理解高斯分布就不理解为什么滤波输出能用均值和方差完整描述。2. 四个必须吃透的概率统计概念2.1 随机变量与概率分布别把观测量当成确定值随机变量是概率统计的起点。它不是一个具体的数而是一个“结果会变、但服从规律”的量。你用同一个传感器在同一位置测一百次每次测出来都不同这时测量值就可以建模为随机变量。随机变量分为离散型和连续型。机器人传感器输出大多是连续型比如角度、速度、距离。连续型随机变量用概率密度函数描述一个常见的误解是“概率密度函数的某个点等于该值的概率”不是的连续型随机变量在某一点的概率其实是0我们关心的往往是“落在某个区间内的概率”。这个细节听起来有点书生气但它直接影响你对噪声的理解我们不会说“误差恰好等于2.0厘米的概率是多少”而会说“误差落在1.5到2.5厘米之间的概率是多少”。实际工程里我们很少直接写概率密度函数而是用一个经验分布或假设分布来近似。比如你用示波器记录IMU静止时的输出画个直方图大概率会看到一个中间高、两边低的钟形。这个钟形曲线就是我们常说的正态分布。随机变量建模的目的是让数学工具能够描述“不确定的东西”没有这一步卡尔曼滤波的推导一步都走不下去。2.2 期望与方差衡量估计的准与稳期望就是随机变量的平均值衡量“中心在哪”方差衡量“围绕中心波动有多大”。一个传感器的测量误差如果期望为0说明它没有系统性偏差不是整体偏大或偏小如果方差很大说明它的单次测量值很分散可信度低。这两个指标刚好对应卡尔曼滤波里的“最优估计”和“置信度”。在卡尔曼滤波中状态估计值$\hat{x}$对应期望误差协方差矩阵对应方差。你每做一次更新本质上就是在调整均值和方差的组合。比如视觉测距的噪声方差比较大那它对最终估计的影响权重就小码盘测距的方差比较小它的影响权重就大。这个权重不是人为拍出来的而是根据每个传感器的方差算出来的这就是卡尔曼增益的本质。有个常见的误区是只关心期望准不准不关心方差。我见过不少同学调卡尔曼滤波只看输出的曲线是否贴近真值却不看协方差矩阵收敛到多少。但协方差不准卡尔曼增益就会算错后续结果自然也不可信。所以期望和方差是两把尺子一把量准度一把量可靠度缺一不可。2.3 协方差多变量之间的牵连关系单变量的方差好理解但机器人系统几乎都是多变量状态。比如云台状态可能包含角度和角速度小车状态可能包含位置和速度这些变量之间并不是独立的。角度变化会带动角速度期望位置变化会带动速度估计这种联动关系就必须用协方差来描述。协方差的正负表示两个变量的变化方向是否一致。协方差为正说明一个变量偏大时另一个也容易偏大为负则相反。把多个变量的方差和协方差凑成矩阵就是协方差矩阵。它的对角线是每个变量自己的方差非对角线是变量两两之间的协方差。这个矩阵在卡尔曼滤波中无处不在预测和更新都会反复操作它。有一个容易踩的坑把协方差矩阵当成完全对角只填对角线方差忽略非对角线。这在变量独立时勉强能凑合但在有强耦合的系统里比如位置和速度非对角线项会显著影响滤波效果。你如果从一开始就完全忽略协方差传播那后面看卡尔曼滤波的状态协方差更新方程就会非常吃力。多想想“角度估计不准时角速度估计是不是也不应该太自信”这比背公式有用得多。2.4 高斯分布卡尔曼滤波的核心假设之一高斯分布也就是正态分布可能是整个概率统计和卡尔曼滤波之间最重要的一座桥。为什么卡尔曼滤波偏爱高斯分布因为高斯分布有良好的数学性质高斯变量经过线性变换后还是高斯分布高斯变量的联合分布和边缘分布也还是高斯分布。这意味着只要初始状态、过程噪声、测量噪声都是高斯分布那么每一步的预测和更新都保持高斯形式计算量可控递归也容易实现。高斯分布由均值和协方差矩阵两个参数完全确定。一维情况下记作$x \sim \mathcal{N}(\mu, \sigma^2)$多维情况下记作$x \sim \mathcal{N}(\mu, \Sigma)$。你不需要死记公式只需要理解均值是分布中心协方差矩阵是分布的“形状”。协方差矩阵越大分布越胖说明不确定性越高协方差矩阵越小分布越瘦说明越有把握。中心极限定理也解释了为什么传感器噪声经常可以被近似为高斯。很多独立小噪声叠加在一起无论单个噪声是什么分布总和都趋向于高斯分布。当然这只是近似实际系统中可能有粗大误差、有偏噪声、非高斯噪声这时候标准卡尔曼滤波的效果会变差需要用鲁棒卡尔曼或扩展卡尔曼来处理。但作为入门先接受高斯假设理解它能做什么、不能做什么比一开始就陷进复杂场景更重要。3. 贝叶斯视角状态估计就是不断修正信念3.1 条件概率与贝叶斯公式条件概率是指在某个条件下另一个事件发生的概率。传感器融合里经常写$p(x|z)$意思是“已知测量值$z$的情况下状态$x$的概率分布”。这个条件概率是整个卡尔曼滤波更新的目标我们想求的就是“看到观测之后状态变成了什么分布”。贝叶斯公式给出了计算后验概率的方法$$p(x|z) \frac{p(z|x)p(x)}{p(z)}$$这里$p(x)$是先验概率比如上一时刻估计出来的状态分布$p(z|x)$是似然概率表示如果真实状态是$x$当前测量$z$有多大概率出现$p(z)$是归一化常数保证概率加起来等于1。最后得到的$p(x|z)$就是后验概率也是融合了预测和观测的结果。我经常跟队友说贝叶斯公式本质上是一个“根据证据修改信念”的框架。你没有测量之前先有一个预测有了测量之后用测量的似然去修正这个预测。测量越可靠修正幅度越大测量越差修正幅度就越小。卡尔曼滤波就是把这个思想落实到了高斯分布上每一步更新都在干这件事。3.2 先验、似然、后验卡尔曼滤波的三种角色把贝叶斯公式对到卡尔曼滤波上你会觉得豁然开朗。预测阶段得到的是先验分布。它完全由上一时刻的估计和系统模型推出来没有任何新传感器信息。比如你知道云台上一帧角度是30度角速度是20度每秒预测下一帧大概在32度但因为这个预测来自不够精确的模型它的方差会比上一时刻更大。观测阶段得到的是似然分布。它描述了如果状态真实值取某个数当前传感器读数出现的概率。比如视觉给了一个测量值31.5度这个传感器的噪声方差已知那么就能写出$p(z|x)$。这个似然函数也是一条高斯曲线中心在测量值附近宽度由测量噪声方差决定。更新阶段计算后验分布就是先验和似然相乘并归一化。两个高斯分布相乘结果仍然是一个高斯分布只是均值和方差都会发生变化而且一定会落在两个分布之间的某个位置。这个位置的偏折方向完全取决于两个分布的方差方差小的那个更有话语权。想象一下你的预测很确定测量很随机那你更应该相信预测反过来测量很精确预测很飘那你就应该跟着测量走。3.3 从贝叶斯公式引出递推估计贝叶斯公式本身只是单次的修正但卡尔曼滤波的精髓在“递推”。每一时刻的后验分布会成为下一时刻的先验分布的基础这个链条一直持续下去不需要把历史数据都存下来。为什么能这样递推因为状态估计问题满足马尔可夫性当前状态包含了历史状态的所有信息只要知道当前状态分布和系统模型就能预测下一个状态只要知道观测量和模型就能更新状态。这种结构让卡尔曼滤波的计算量恒定特别适合单片机实时运行。你在RM电控里跑卡尔曼滤波每帧的计算量不会随着时间增长就是这个原因。从概率统计的角度理解递推还有一个实际价值它解释了为什么卡尔曼滤波能够“越用越准”。初始时协方差比较大系统还不太相信估计随着不断接收观测后验协方差逐渐缩小状态估计被约束得越来越紧。如果发现协方差一直不变说明系统已经达到稳态卡尔曼增益会收敛到一个常数。这就是工程上“固定增益卡尔曼滤波”的由来。4. 用最小均方误差理解最优估计4.1 估计误差与代价函数说完了贝叶斯视角你可能觉得卡尔曼滤波只是在“算概率”。但其实它还涉及一个最优性问题那么多可能的估计值为什么最后选均值因为在均方误差准则下条件期望就是最优估计。均方误差的定义是真实状态$x$与估计值$\hat{x}$之差的平方的期望即$E[(x-\hat{x})^2]$。这是一个代价函数衡量估计结果偏离真值的平均程度。为什么用平方而不用绝对值因为平方函数处处可导数学上处理方便而且它会加大对大误差的惩罚符合工程直觉宁可让误差均匀一点也不要偶尔出现离谱偏差。最小均方误差估计就是在给定观测$z$的条件下选择一个$\hat{x}$使得上述期望最小。最小化的结果是$\hat{x} E[x|z]$也就是条件期望。这个结论从概率统计的角度看非常干净你不需要假设高斯分布只需要定义代价函数最优估计就是后验分布的均值。卡尔曼滤波器输出$\hat{x}$本质上就是高斯后验的均值。4.2 高斯假设下的最优估计在高斯假设下最小均方误差估计和最大后验估计是等价的。原因是高斯分布的均值、中位数、众数都在同一个点所以不管用哪种准则最优估计都是那个中心点。这也是卡尔曼滤波公式看起来“简洁”的原因之一它不需要处理复杂的积分只需要维护均值和协方差矩阵就够了。但要注意这个等价关系只在高斯假设下成立。如果后验分布是非高斯的比如多峰分布条件期望可能会落在两个峰之间这个点作为估计值其实毫无意义。这也是为什么很多工程场景不能硬上标准卡尔曼滤波而要用粒子滤波等更复杂的方法。理解了这一点你就明白为什么概率统计基础对判断算法适用范围这么重要。在卡尔曼滤波里更新阶段计算后验分布时本质上就是把先验高斯和似然高斯相乘。这条乘法做完之后后验均值可以写成$$\hat{x}{post} \frac{P{pred}}{P_{pred}R}z \frac{R}{P_{pred}R}\hat{x}_{pred}$$这个式子虽然是一维形式的但它把卡尔曼增益的思想完全暴露出来预测方差$P_{pred}$和测量方差$R$的比值决定了你更相信谁。$P_{pred}$相对越大越相信测量$R$相对越大越相信预测。4.3 一维卡尔曼滤波的最小实现为了让上面的概念落地这里给一个最简单的一维卡尔曼滤波不搞矩阵只在代码里体现概率统计思想。假设你要估计一个云台的当前角度角度满足匀速转动模型陀螺仪每个控制周期给出角速度编码器或视觉给出带噪声的角度观测。状态方程简化成一个变量角度$x$角速度作为输入$u$。预测方程就是$$x_{pred} x_{prev} u \cdot dt$$ $$P_{pred} P_{prev} Q$$这里$Q$是过程噪声方差代表模型预测的不确定性。更新方程是$$K \frac{P_{pred}}{P_{pred}R}$$ $$x_{new} x_{pred} K(z - x_{pred})$$ $$P_{new} (1-K)P_{pred}$$这套公式不长但里面的每一个变量都可以从概率角度解释。$P$是估计方差$Q$是过程噪声方差$R$是测量噪声方差$K$是卡尔曼增益。$z - x_{pred}$叫新息表示实际测量和预测之间的差异方差越大这个差异就越不可信$K$就越小对估计的修正越少。这正好对应贝叶斯公式里的“用似然修正先验”思想。5. 实战演练用 Python 搭建一个一维位置估计器5.1 问题建模与数据生成理论讲得再多不如动手跑一把。我们用Python实现一个模拟场景小车在直道上运动我们只能拿到两个信息一个是加速度计/陀螺仪输出的速度积分结果另一个是编码器或视觉输出的位置观测。前者更新频率高但会漂移后者噪声大但不会漂移。这样设置很像RM比赛里融合IMU和视觉测距的场景。先模拟真实状态和测量数据过程噪声标准差设为0.1米测量噪声标准差设为1米。import numpy as np np.random.seed(42) dt 0.1 # 控制周期 100ms total_time 10.0 # 模拟10秒 steps int(total_time / dt) # 真实运动模型初始位置0速度1.2m/s true_pos 0.0 true_vel 1.2 q_std 0.1 # 过程噪声标准差 r_std 1.0 # 测量噪声标准差 true_positions [] measurements [] for i in range(steps): # 真实位置受随机扰动 true_pos true_vel * dt np.random.normal(0, q_std) # 观测位置含测量噪声 z true_pos np.random.normal(0, r_std) true_positions.append(true_pos) measurements.append(z)这段代码里我们人为制造了两层随机性一层是真实运动本身受到扰动另一层是观测噪声。注意在实际系统里真实状态你是不知道的这里生成的数据只是为了对比滤波结果。很多初学者会忽略“过程噪声”和“测量噪声”的区别把两者混为一谈这是调参时的大忌。5.2 手写卡尔曼滤波的核心更新接着我们手写一维卡尔曼滤波不调用现成库。初始状态我们不知道真实位置所以设$x0$初始误差方差$P$设得比较大比如5.0表示“刚开始非常不确定”。过程噪声方差$Q$取$0.01$对应标准差0.1的平方测量噪声方差$R$取$1.0$对应标准差1.0的平方。x 0.0 P 5.0 Q q_std ** 2 R r_std ** 2 filtered_positions [] kalman_gains [] for i in range(steps): # 预测 x_pred x true_vel * dt P_pred P Q # 更新 K P_pred / (P_pred R) z measurements[i] x x_pred K * (z - x_pred) P (1 - K) * P_pred filtered_positions.append(x) kalman_gains.append(K)这里的预测用了真实速度实际工程里可能没有这么准确的模型但作为演示已经足够。你可以打印前几步的$K$会发现刚开始$P$很大$K$接近1表示我们更相信第一次测量随着滤波收敛$P$逐渐缩小$K$慢慢稳定到一个小于1的值开始在预测和测量之间取一个固定权重。我建议你把这个代码跑一下记录三组数据真实位置、测量位置、滤波位置。你会看到测量曲线在真值附近剧烈抖动滤波曲线要平滑得多而且滞后感不明显。这是因为卡尔曼滤波在每一帧都做了“预测-修正”两步把高噪声观测和相对稳定的运动模型结合了起来。5.3 结果观察与参数调参跑完代码后你可以试着调三个参数初始$P$、$Q$和$R$。初始$P$只影响前几步收敛之后基本可以忽略所以不要花太多时间纠结。$Q$和$R$的比值才是关键。调节方向效果直观解释$Q$偏大滤波更相信测量曲线更跟手但更抖认为模型不可靠预测方差大$Q$偏小滤波更平滑但响应变慢认为模型很准预测权重高$R$偏大滤波更平滑但滞后明显认为测量噪声大不敢信测量$R$偏小滤波更接近测量抖动变大认为测量很准紧贴观测很多同学上来就把$Q$调到0以为模型精度高结果传感器测量稍微一跳滤波就不理它了。实际中模型的误差来源很多速度估计不准、控制周期抖动、忽略了加速度变化这些都该折算进$Q$。反过来如果你把$R$调得特别小滤波输出方差会很小看着稳但一旦测量出现粗大误差会被错误地当成真实状态导致估计瞬间跑飞。调参不能只看曲线光不光滑还要看动态响应。在RM赛场上云台快速转向时如果你发现滤波后的角度跟不上真实角度多半是$Q$调太小了。相反静止时滤波输出还在抖说明$R$可能偏小。用“静止看方差动态看延迟”这个口诀去调会快很多。6. 初学者最容易踩的坑6.1 把方差和噪声方差混淆这是我在带新人时遇到最多的问题。有人说“我的传感器方差是0.1”然后直接把$R$设成0.1结果滤波纹丝不动。原因很简单传感器数据手册上给的“方差”往往是AD量化方差或原始噪声方差而你实际使用中的测量经过滤波、标定、单位换算后噪声统计特性已经变了。正确做法是用实际数据去统计。让传感器静止记录几百个输出值计算样本方差再根据这个值去设定$R$。角度传感器的单位通常是度如果数据手册给的是弧度你直接填进去那$R$会差好几个数量级。概率统计里最基础的经验就是“样本统计量要和实际问题对齐”单位、量纲、坐标系都不能忽略。6.2 协方差矩阵初始化拍脑袋多维卡尔曼滤波中初始协方差矩阵$P_0$如果设得太小滤波会“早熟”对后续测量不敏感设得太大前几步会剧烈修正输出可能冲出合理范围。很多人的处理方式是在调试里试一个看起来差不多的对角阵但从不关注非对角线项。我的建议是初始化时宁可让$P_0$大一点。因为卡尔曼滤波的收敛过程本质上就是协方差慢慢减小的过程一开始的不确定不会影响稳态。真正麻烦的是矩阵的非对角线比如位置和速度的协方差初始为0但在某些系统里它们强相关。忽略这个相关项初期估计可能震荡得很厉害。你可以用仿真数据先跑一遍对比不同初始化对角项和非对角项下的收敛表现再决定取值。6.3 Q和R的比例关系被当成玄学很多人都说$Q$和$R$靠感觉调其实它们有明确概率意义。它们代表你对模型和测量的信任程度但真正影响滤波行为的是比值不是绝对值。如果你把$Q$和$R$同时放大10倍卡尔曼增益不变滤波结果几乎一样。所以翻来覆去调两个数的绝对值毫无意义。调参的系统方法是用真实数据离线调。把比赛或测试时录下的传感器数据保存下来用不同的$Q$、$R$组合重放对比滤波输出和参考真值。这个过程可以自动化用网格搜索或简单遗传算法都可以。每次比赛前花半小时做一次离线调优比在赛场上拍脑袋试参数靠谱得多。6.4 非线性系统的误区标准卡尔曼滤波只适用于线性系统。如果你的模型是非线性的比如用四元数表示姿态或者测量方程里有三角函数直接用标准卡尔曼滤波就是把非线性函数硬当成线性来算方差传播必然出错。这时候你需要的是扩展卡尔曼滤波或无迹卡尔曼滤波。它们背后的概率统计思想还是一样的只是多了“如何近似非线性函数下的概率分布”这一步。扩展卡尔曼滤波用一阶泰勒展开无迹卡尔曼滤波用一组采样点来近似分布。理解了高斯分布和协方差传播再看这两种方法本质上就是“换一种办法算均值和协方差的传播”。最后再分享一个我个人的心得学卡尔曼滤波别急着背公式先把随机变量、高斯分布、协方差、贝叶斯公式这些概念在纸上画一画。你可以自己手写一维滤波跑通再去碰矩阵形式的公式那时候你会发现自己不再是“调参数的工具人”而是真的能判断滤波哪里出了问题。下一篇合集我们接着往下走把状态空间模型和卡尔曼滤波的完整推导拆开到时候概率统计这层地基的作用会越来越明显。