在很多时候我们迫切想要知道真相：
- TA到底喜不喜欢我？
- 股价上涨是趋势还是短期波动？
- 老板到底对我满不满意？
- 这个人是真的可靠，还是偶尔表现好？
- 孩子成绩到底是进步了，还是运气好？
- 我体重真的变轻了，还是水分波动？
但现实给我们的信息是嘈杂的、片段的、带噪声的
- 一次相处愉快不能说明是真爱；
- 一次考试不能代表能力；
- 一天体重不能代表趋势；
于是我们会本能地做一件事：
- 不只看一次结果
- 结合过去的情况
- 判断这次变化是不是合理
- 在“历史经验”和“当前信息”之间做权衡
慢慢逼近那个真相。
而卡尔曼滤波做的事情，其实和我们一模一样，
它不是神秘的公式，它只是把“人类逼近真相的方式”，写成了数学。
卡尔曼滤波本质是在随时间连续变化的系统中，每一次推演真实状态时，用“预测不确定性”和“测量不确定性”当作权重，在“预测值”和“测量值”之间做动态加权平均。
其中：“动态”指预测的“不确定性”和测量的“不确定性”不是固定的，而是每一时刻都在变化。
想象一个场景：徒步爬山
你知道自己步行速度大概是1m/s。
10秒前你在 A 点，所以这会儿大概率在 A+10米 地方—— 这是预测。
GPS 告诉你在 B 点 —— 这是测量。
地图上B点离A点大概30米左右
你就会想：
“我10 秒不可能走那么远，GPS 可能在抖，但我好像今天是走的比较快一点呢”
于是你在：
- 预测的位置（不准确）
- GPS 给的位置（也不准确）
之间取一个更合理的折中值，然后以这个值为准继续探路爬山。
卡尔曼滤波的目标是在多个不确定因素中逼近隐藏的真实状态。
现实世界中的观测数据往往充满波动和噪声，就像信号里的杂波一样，卡尔曼滤波通过不断修正和加权，过滤掉这些不稳定的波动，保留更接近真实的部分，从这个角度看它就是一种“滤波”器，叫卡尔曼滤波也名副其实。
接下来，我们一起看看卡尔曼滤波每一步演化是怎么做的，比如如何得到“预测不准确定”和“测量不准确性”，如何动态更新权重。
开始了~
在一维情况下， 每一次卡尔曼滤波的演化
其中：
是在t时刻的我们得到的”真实状态值“。
是在t时刻的预测状态，它是使用一个预测模型，根据t-1时刻的状态得到。
是在t时刻的观测值。
是权重，它取值范围是[0~1]，它代表信任“预测”的比例。
那这个“信任比例”是怎么得到的呢？
首先看下预测的信任度如何计算——
我们判断“此次预测”有多值得信任，是基于“预测模型”的历史表现进行的
到这里，有人问什么是“预测模型”。
“预测模型“就是通过历史数据找规律，从而对未来结果做出定量判断的数学模型。
比如前面讲爬山预测位置可以建立模型如下（简化为朝一个方向的一维模型）：
有了预测模型，那它的信任度如何量化呢？
信任度的另一面面就是不确定性，不确定性越大那就信任度越低，工程上一般用不确定性来衡量信任度。
不确定性衡量就用到了《卡尔曼滤波前置知识》提到的方差，方差越大越不值得信任。
比如A同学测量了自己10次步行速度，平均速度是1米/秒，但是方差是0.1；
B同学同样测量了10次步行速度，平均速度也是1米/秒，但是方差是1；
那么同样走了10秒，A估算出来自己走了10米的可信度就比B高了很多。
那信任度是一成不变的吗？
非也，信任度会随着自身状态和场景而有所变化。
比如A同学想让自己预测速度更准确，就仔细研究了其中关键因素，他发现：
在平地上很容易保持1米每秒的速度，方差非常小是0.05；
但是在山地上，他就很难保持这个速度，方差变成了0.9；
那么下次他评估自己行进多远的时候就会考虑上这个地形因素。
当找到了影响信任度的因素，“预测信任度”就可以根据这些因素的变化进行更加精准的动态评估计算了。
“测量信任度”的计算也是如此，我们需要利用测量仪器供应商提供的说明和相关知识去发掘影响“测量信任度”的各种因素。比如GNSS定位中卫星数量、信号强度等~
注意：只有在量纲一致的前提下，卡尔曼增益（就是指这种寻找的最优折中比例）才具有物理意义。
在卡尔曼滤波中信任度计算用到方差和协方差必须描述同一物理量，并且具有相同单位，例如在定位场景下，若状态是位置（单位：米），则对应的协方差单位必须是米²。
卡尔曼滤波的每一步都要计算“我对每一项的信任度有多少”，我对综合结果的信任度又是多少，然后把这步的结果当作下一步预测的起点。
OK，让我们看看一个完整流程：
上一时刻状态+不确定性
↓
【预测】
用模型预测下一步状态
评估预测数据不确定性
这时候不确定性会变大
↓
【测量】
传感器给数据
评估测量数据不确定性
↓
【融合】
根据预测和测量的不确定性计算权重
根据权重得到卡尔曼滤波融合计算参数，更新所有状态
根据权重来修正最终结果的不确定性，这时候不确定性会变小
↓
进入下一轮
这里重点谈谈”状态“这个概念。
状态（State）是在当前时刻，能够完整描述系统未来演化所必需的最小变量集合。
只要知道这组变量，不需要历史数据，就能用物理规律预测系统下一时刻的样子。
状态 = 描述系统现在 “到底是什么样” 的最少信息。
它必须有三个特点：
- 完备性：知道它，就能预测未来，不用翻旧账。
- 最小性：不多加无用变量，够用就行。
- 马尔可夫性：下一时刻只由当前状态决定，和更早历史无关。
在建型时候需要尽力找准状态，因为一旦一个信息变量成为状态，它就具备了“被修正的资格”，就能在卡尔曼滤波迭代过程中不断的得到修正。
OK 理论讲解完毕，接下来用一个例子来具体看看卡尔曼滤波是怎么使用的。
我们就用车机定位的例子看看吧
在整个定位系统运行前，首先要做的是
1、确定状态
状态 S = [
位置 l
速度 v
IMU 加速度偏差 bias，简写为ba
]
其中：l、v、bias都是多维度数据
2、建立每一步的预测模型
1. 从 IMU 获取到加速度 a
2. 去掉偏差 bias：a = a - ba
3. 更新速度：v = v + a * dt
4. 更新位置：l = l + v * dt
定位系统开始运行
车机定位系统是随着时间一直进行的，我们就t时刻是怎么使用卡尔曼滤波进行状态评估的。
当然这个的前提是：我们已经知道t-1时刻的状态，也知道t-1时刻对这个状态的不确定性。
用数学表达就是，目前已知是t-1时刻状态是：
t-1时刻的不确定性（用协方差矩阵表示）是：
1 、【预测】（Prediction）
根据刚才建立的运动模型，我们首先在 t 时刻做一次状态预测（t和t-1时刻的间隔是dt）：
1）读取 IMU 加速度：
2）更新加速度→更新速度：
3）更新位置
4）更新偏移量
这里需要注意叫随机漂移，影响位置和速度的是上一个时刻的偏移，然后在t时刻 bias 发生随机变化
。
把2带入3，得到状态转移方程组
用矩阵表示就是：
简化下：
其中叫转移矩阵，
是一个随机值，上述公式可以认为最接近真实世界的数学模型，或者说我们认为真实世界就是按照这个规律演化的，这也是我们去估算不确定性的前提。
由于是未知的，所以实际工程中预测值更新用到的公式是：
假设真实状态是
预测状态是
那么误差就是
带入公式得到
得到真实值和预测值的误差公式了，那么预测值的不确定性计算就来了，就是计算协方差矩阵（协方差矩阵是包含方差的，请参考协方差矩阵一文）
经过若干公式推导，这部分可以问文心、豆包、元宝都很容易得到，就不展开了
最后可以得到
其中
其中n(t) 是零偏的随机变化率，对他进行积分后就可以得到厂商提供的零偏稳定性。
所以最终预测不确定性计算公式是
其中就是厂商或者长期实验过程中得到的零偏稳定性，可以参考《IMU 的零偏稳定性》一文。
到这里也就是说，只要拿到状态转移方程（其实就是预测模型参数）以及零偏误差，就可以得到每一步骤的预测不确定性了。
2、【测量】
这一个步骤就是去获取测量的值，以及评估它的不确定性（注意要跟预测使用同一个量纲）。
在真实系统中，测量往往不是完整状态，所以我们用一个矩阵描述传感器能看到状态的哪一部分
其中x是状态量，H是映射矩阵，比如:
表示，只能测量到x的第一个状态，就是位置。
既然
那么在状态 对应的测量应该是
，真实测量是
，我们知道测量是有误差的，这个误差就是
，一般叫它改变量或者残差，它表示预测跟测量对比错了多少。比如
- 预测位置 = 100m
- GPS测量 = 105m
- z − Hx⁻ = 5m
就是说这个预测位置比实际测量位置少了5米，那卡尔曼滤波就会考虑往测量方向修正一点，但是修正多少呢，这就又涉及到测量的不准确性了。
测量的不准确性就是测量工具的精度，比如位置的不确定性就是gps精度的平方，推理过程类似上面不再重复，可以咨询豆包、文心、千问、gpt等AI，用数学表示就是测量的不确定性R为
3、【融合】
卡尔曼滤波本质是在随时间连续变化的系统中，每一步状态演化时，用“预测不确定性”和“测量不确定性”当作权重，在“预测值”和“测量值”之间做动态加权平均。
在一维的时候数学公式很容易看出来的确是这样，
但是如果状态是多维的+测量不是完整状态，卡尔曼思想在多维下的公式就变成了
最终的状态不是直接相信测量，也不是完全相信预测，而是在预测状态的基础上，根据测量和预测状态的误差进行修正，当状态是一维情况下，就退回上面的公式了。
这个公式中的K叫做卡尔曼增益，计算公式如下
卡尔曼滤波工程学表述就是：先用物理模型预测未来，再让传感器告诉我们预测错了多少，然后按可信度修正。
当 确定后，我们就可以对预测状态进行修正。
首先更新状态：
其中：
是预测状态
是测量残差（测量与预测之间的差异）
决定我们修正多少
直观理解就是：
先相信预测，再根据测量“纠偏”。
如果测量非常可靠（R 很小），那么 会变大，系统会更多向测量靠拢。
如果测量噪声很大（R 很大），那么 会变小，系统会更多相信预测。
但状态更新之后，还有一个同样重要的步骤：
更新系统对自身状态的“不确定性”。
数学表达是：
其中：
是预测不确定性
是融合后的不确定性
直觉上理解：
预测阶段，不确定性会增加
融合测量后，不确定性会减少
因为我们获得了新的信息。
到这里，一轮卡尔曼滤波就完成了。
__
总结
卡尔曼滤波，其实是一种“持续修正世界模型”的方法
回头看整个过程，你会发现一件很有意思的事情。
卡尔曼滤波做的事情，并不是简单的“滤波”，
它其实是在不断构建一个关于世界的内部模型。
这个模型包含两样东西：
我认为世界是如何演化的（预测模型）
我认为传感器有多可靠（测量模型）
每当时间向前走一步：
1️⃣ 我先用 物理规律 预测未来
2️⃣ 再用 传感器数据 看看测量偏离了多少
3️⃣ 最后根据 双方可信度 调整自己的认知
于是系统的认知就在不断循环：
预测世界
↓
观察世界
↓
修正认知
↓
得到新的世界模型
然后再继续下一轮。
这其实就是一种机器版本的“认知更新过程”。
从更高的角度看
如果把卡尔曼滤波再抽象一层，它其实是在解决一个非常普遍的问题：
在一个充满噪声和不确定性的世界里，如何逐步逼近真实状态。
现实世界几乎所有系统都面临这个问题：
自动驾驶要判断 车在哪
飞机要判断 自己姿态是什么
机器人要判断 自己在地图上的位置
金融系统要判断 价格的真实趋势
医疗设备要判断 人体真实信号
所有这些问题都有三个共同特点：
1️⃣ 世界在不断变化
2️⃣ 观测永远带噪声
3️⃣ 真相永远是隐藏的
卡尔曼滤波给出的答案是：
不要试图一次看清真相，而是让系统在时间中不断修正自己。
一句话理解卡尔曼滤波
如果用一句话总结：
卡尔曼滤波就是：用物理规律预测未来，用观测修正预测，并根据不确定性动态决定相信谁。
或者更工程一点：
先用模型猜，再用传感器纠偏。
很多人第一次看到卡尔曼滤波，会觉得它是一个复杂的数学公式。但如果理解之后就会发现：
它其实非常朴素，它只是把人类在现实世界中一直在做的一件事：
- 根据经验预测
- 根据现实修正
- 根据可靠性判断相信谁
写成了数学。
而这正是卡尔曼滤波伟大的地方：它把“经验判断”变成了可以计算的数学。
此外，卡尔曼滤波之所以应用广泛，还有一个重要原因是它本质上是一个可扩展的框架。
在整体系统中，我们可以设置统一的状态演化模型和噪声参数；而当某些特殊场景被深入研究之后，又可以在这个统一框架下，对相关模型或参数进行针对性的调整，从而提高系统在特定情况下的表现。
也正因为如此，卡尔曼滤波才能在从航天导航、自动驾驶到机器人定位等各种复杂系统中长期发挥作用。
