企业官网建设流程全解析

做车辆纵向状态估计，最头疼的往往不是算法本身多高深，而是怎么把纸上写好的公式变成一套能落地、能验证、改参数还不用重启仿真的Simulink模型。卡尔曼滤波这个老牌算法，在车速和位移估计里可以说是“常规武器”，但要真正跑通并且估计得又稳又准，建模、离散化、噪声协方差整定每一步都有坑。这篇文章我打算从为什么要用卡尔曼滤波讲起，把状态方程怎么列、五大公式每一步在干什么、Simulink里怎么用MATLAB Function实现全流程讲透，最后附上我自己调参和排错的经验。适合正在做车辆状态估计、智能驾驶纵向控制，或者刚接触卡尔曼滤波想找个完整案例入手的工程师和研究生，照着搭一套模型，十分钟内应该能出第一版结果。

1. 为什么偏偏用卡尔曼滤波做纵向位移和速度估计

1.1 单一传感器根本不够用

车辆纵向控制里，最核心的两个量就是纵向位移和纵向速度。你可能觉得这有什么好估计的，装个轮速传感器、装个GPS不就有了？真正做工程的时候你会发现每个传感器都有“性格缺陷”。

轮速计测车速，问题出在轮胎和地面接触的滑动。急加速的时候驱动轮滑转，轮速比实际车速高；急制动的时候车轮抱死或者接近抱死，轮速比实际车速低。你要是直接把轮速当车速，ABS和TCS这些纵向控制系统拿到的反馈就是错的。GPS定位能直接给出位置，但更新频率低，一般就10Hz左右，城市高架下、隧道里还会丢星，位置跳变是常事。加速度计倒是可以高频测量纵向加速度，但它有零偏，积分一次出速度，再积一次出位移，几分钟就开始飘。

这就是典型的多传感器冗余场景，每个传感器都有噪声和失效模式，单独用哪个都不放心。卡尔曼滤波干的事情，就是把这些传感器的信息按“谁更可信谁权重更大”的原则融起来，最后给出一套对位移和速度的估计，同时还能告诉你这个估计值精度有多高，也就是协方差。

1.2 卡尔曼滤波解决的核心矛盾

卡尔曼滤波的本质不是“智能算法”，它就是一个递推的最优状态估计器，核心思想是两条信息源互相校验：

一条是“模型预测”。我根据上一时刻的状态和当前控制输入，用运动学模型推一下这一时刻车辆大概在哪、速度大概多少。但模型不准，它有过程噪声。另一条是“传感器测量”。轮速计测出来的速度和GPS测出来的位置，直接反映车辆状态，但它们也有测量噪声。

滤波器的任务，就是在这两条信息之间找一个加权平均，权重由噪声协方差决定。模型误差大，就多信一点测量；测量噪声大，就多信一点模型。整个过程是递推的，每来一个新测量就更新一次，不需要缓存历史数据，计算量很小，非常适合车载ECU或者嵌入式控制器实时跑。这个特点，是它比批量最小二乘、比离线平滑更适合在线控制场景的根本原因。

2. 状态空间建模：把物理问题变成数学问题

2.1 选一个简单但够用的运动学模型

做卡尔曼滤波第一步是列状态方程。很多新手上来就想用复杂的七自由度车辆动力学模型，没有必要。纵向位移和速度估计的核心是运动学关系，不是动力学关系，所以用一个匀加速运动模型就足够了。

定义状态向量为：

x = [s; v]

其中 s 表示纵向位移，v 表示纵向速度。系统的控制输入 u 取纵向加速度 a，也就是IMU测到的那一路信号。

连续时间状态方程可以写成：

ds/dt = v dv/dt = a

这就是最朴素的一阶运动学关系：位移的导数是速度，速度的导数是加速度。写成矩阵形式：

dx/dt = [0 1; 0 0] * x + [0; 1] * u + w

这里 w 是过程噪声，它代表模型里没考虑到的扰动，比如加速度测量误差、路面坡度引起的额外加速度、模型简化带来的偏差等等。

在Simulink里做数字仿真，状态方程必须离散化。用前向欧拉法，采样周期为 dt，可以写成：

x(k) = A * x(k-1) + B * u(k-1) + w(k)

其中：

A = [1 dt; 0 1] B = [0.5*dt^2; dt]

注意 B 矩阵为什么是 [0.5dt^2; dt] 而不是 [0; dt]？这里其实是连续方程精确解的形式。位置的变化不仅包含速度项 vdt，还包含加速度引起的 0.5adt^2 项。前向欧拉近似很多时候只取 v*dt，但如果加速度比较大或者 dt 比较长，忽略二次项会引入明显误差。我一般直接采用带二次项的形式，公式不复杂，精度还能好一截。

2.2 观测方程怎么设计最合理

观测方程描述的是“传感器测到了什么”和“状态之间有什么关系”。在这个场景里，最常见的两种接法：

• 以轮速计测量的速度为观测，观测矩阵 H = [0 1]。好处是简单直接，轮速信号车速上最容易得到的信号，坏处是轮胎打滑时轮速不等于车速，这个误差会被当测量噪声处理。
• 以GPS定位的位置为观测，观测矩阵 H = [1 0]。好处是位置是绝对量，不存在积分漂移问题，坏处是更新频率低、偶尔跳变。

那到底用哪个？我的建议是能融合就融合。把两个观测都怼进去，观测向量 z = [v_wheel; s_gps]，对应的观测矩阵就变成：

H = [0 1; 1 0]

测量噪声协方差 R 也变成2x2矩阵，对角线分别是轮速噪声方差和GPS位置噪声方差。滤波器会自动根据R矩阵分配权重，哪个传感器质量差，它在增益计算里的贡献就自动变小。

这里有一个很关键的工程认知：卡尔曼滤波只能融合“带噪声的测量”，不能消除“系统性偏差”。打个比方，轮速计在急刹车时整个信号偏低，这不是零均值噪声，而是偏差。卡尔曼滤波会把这种偏差当成真实状态变化去跟踪，结果车速估计就跟着偏了。所以如果你知道当前处于强滑移工况，要么在滤波器外面做工况判断，要么把轮速噪声方差 R 临时调大，告诉滤波器“我现在不太信任轮速”。这种“带先验信息的参数调度”在实际项目里非常常见。

3. 卡尔曼滤波五大公式到底在算什么

3.1 预测和更新两步走

卡尔曼滤波整个流程可以压缩成五条公式，分成两步：预测（时间更新）和校正（测量更新）。

预测部分，用上一时刻的最优估计和控制输入推算当前时刻的先验估计：

x_pred = A * x_est + B * u P_pred = A * P_est * A' + Q

这里 P 是状态协方差矩阵，对角线元素表示每个状态估计的方差，非对角线表示状态之间的相关性。P_pred 表示“模型预测这一步带来的不确定性”。

校正部分，用传感器测量来修正先验估计：

K = P_pred * H' * (H * P_pred * H' + R)^(-1) x_est = x_pred + K * (z - H * x_pred) P_est = (I - K * H) * P_pred

K 叫卡尔曼增益，它本质上是一个权重矩阵，决定了“模型预测”和“传感器测量”各信多少。z - H*x_pred 叫新息，也就是“测量值和模型预测值之间的差”。新息越大，说明模型预测和实际测量差得越远，要么是模型误差大，要么是状态真的变了，滤波器通过 K 来决定用多大幅度去修正。

用一个生活类比来理解：你开车从家出发去公司，导航告诉你按当前速度你大概在哪个位置，这对应“模型预测”。你抬头看见路边一个地标，确认了实际位置，这对应“传感器测量”。如果你最近这段路很堵，导航模型的误差很大，你会更相信地标；如果你视力不好，地标看得模模糊糊，你会更相信导航。卡尔曼滤波的 K 就是自动在算这个“更相信谁”的比例。

3.2 Q 和 R 的物理含义必须搞清楚

Q 是过程噪声协方差矩阵，它表示的是你对“模型预测”的信任程度。Q 越大，说明你觉得模型越不靠谱，滤波器就会更依赖测量，增益 K 会变大，估计结果跟踪快但噪声大；Q 越小，说明你觉得模型很准，滤波器会倾向于相信模型，结果平滑但对突变的响应变慢。

R 是测量噪声协方差矩阵，它表示你对传感器的信任程度。R 越大，说明传感器噪声越严重，滤波器会更依赖模型，增益 K 变小；R 越小，说明传感器很准，滤波器会大胆跟随测量。

这里有个新手容易绕进去的误区：Q 和 R 不是独立起作用的，起作用的是它们的比值。滤波器行为基本由 Q/R 这个比值决定，你同时把 Q 和 R 放大十倍，稳态结果几乎不变。所以调参的时候，可以先固定一个，只调另一个，会省很多事。

对于我前面提到的运动学模型，Q 矩阵本身也是2x2的，对应位置和速度的过程噪声。如果加速度噪声方差是 q，那么离散化之后的理论形式是：

Q = q * [dt^3/3 dt^2/2; dt^2/2 dt]

这个形式包含了噪声从加速度积分到位移和速度的传播过程。实际工程里为了方便，很多人直接取对角形式，比如 Q = diag([0.01, 0.1])，效果也说得过去。我的建议是：开始先用对角形式跑通，理解每个参数的影响，再换成理论形式精调。

4. Simulink模型搭建：从框图到跑通

4.1 模型总体架构怎么搭

Simulink模型的整体结构分成三块：真实被控对象、传感器测量模块、卡尔曼滤波器。说白了，就是要有一个“真值”用来对比验证，再有一个“带噪声的观测”喂给滤波器，最后看滤波器输出能不能逼近真值。

被控对象部分很简单，不需要什么汽车动力学插件，就是两个积分器串联：加速度 u 经过积分器1得到速度 v，速度经过积分器2得到位移 s。这里你可以给加速度加一个小的过程噪声信号，模拟实际扰动。传感器测量模块就是给真实速度叠加一个高斯白噪声，模拟轮速计的测量输出；也可以另加一路给位置叠加噪声，模拟GPS位置输出。

卡尔曼滤波器部分，推荐用 MATLAB Function 模块实现，不推荐用一大堆 Gain、Sum 模块搭公式，因为公式里面有矩阵运算和递推，用状态模块搭极其容易出错，可读性也差。MATLAB Function 块里直接用矩阵运算，清爽得多。

整个仿真建议设置固定步长，比如步长0.01s，求解器用ode4，这样和滤波器内部的离散模型采样时间一致，避免步长不匹配引入额外误差。

4.2 MATLAB Function 模块写卡尔曼滤波

下面是我实际在用的一个实现，比较简洁，直接在Simulink的MATLAB Function模块里运行。

function [s_est, v_est, p11, p22] = kf_vehicle(u, z, dt) persistent x P if isempty(x) x = [0; 0]; % 状态初值：位移和速度 P = eye(2); % 协方差初值，不能给零矩阵 end % 状态转移矩阵和控制矩阵 A = [1 dt; 0 1]; B = [0.5*dt^2; dt]; % 观测矩阵：这里只观测速度 H = [0 1]; % 过程噪声协方差 q = 0.5; Q = q * [dt^3/3 dt^2/2; dt^2/2 dt]; % 测量噪声协方差 R = 1.0; % 预测 x_pred = A * x + B * u; P_pred = A * P * A' + Q; % 更新 K = P_pred * H' / (H * P_pred * H' + R); x = x_pred + K * (z - H * x_pred); P = (eye(2) - K * H) * P_pred; % 输出 s_est = x(1); v_est = x(2); p11 = P(1,1); p22 = P(2,2); end

这段代码里几个关键点值得说一下。

• persistent 变量用来保存上一时刻的状态估计 x 和协方差 P，这是递推算法的核心。Simulink仿真过程中，persistent变量会在整个仿真时间内保留，不会每个时间步清零。
• 初始协方差 P 不要设成零矩阵。如果设成零，滤波器会认为初始状态完全准确，开始阶段几乎不信任任何测量，估计值会长时间“赖”在初值附近不动。一般给单位矩阵或者稍大一点的对角矩阵都可以。
• Q 用的是理论离散形式，注意它的单位。q 的量纲是加速度噪声方差，单位一般是 (m/s^2)^2，也就是 m^2/s^4。如果你用的是对角简化形式，需要把 q_s 和 q_v 的量纲搞清楚，否则会出现“明明调了参数但滤波器行为完全没变化”的情况。
• 代码里用的是普通除法 /，在MATLAB里对于矩阵运算等价于右除，也就是乘以逆矩阵。为了防止新手混淆，也可以写成 K = P_pred * H' * inv(H * P_pred * H' + R)，但 inv 形式数值稳定性稍差，实际中我更喜欢用 /。

4.3 输入信号和测量信号怎么接

仿真模型里，加速度 u 直接从被控对象的加速度源引出，这就相当于理想情况下滤波器拿到了真实的控制输入。实际项目里IMU测到的加速度是有噪声甚至有零偏的，你可以在 u 的通路上加一个常值偏移和噪声，观察估计结果会有什么变化，这会是一个非常直观的鲁棒性测试。

轮速计的测量 z 从被控对象的速度输出引出，叠加上高斯噪声。在Simulink里可以用 Sources 库里的 Random Number 或者 Band-Limited White Noise 模块。注意 Random Number 模块需要设置 Sample time，我一般设成和仿真步长一致。做对比实验的时候，记得把种子 seed 固定，否则每次运行噪声序列不一样，调参时很难判断是参数变了还是随机噪声变了。

滤波器模块的输入有三个：加速度 u、测量速度 z、采样时间 dt。dt 你可以用常量模块给0.01，也可以直接用仿真步长。我的习惯是显式地给 dt 参数，这样以后改成变步长或者多速率采样，不用改代码逻辑。四个输出分别是估计位移、估计速度、位移协方差、速度协方差，前两个进 Scope 画图，后两个可以接显示器模块实时观察滤波器的自评估结果。

4.4 对比验证：纯积分和卡尔曼滤波的差距

搭好模型之后，我强烈建议做一组对比：把真实位移 s_true、纯积分位移 s_int（直接对测量速度积分）、卡尔曼滤波估计位移 s_est 三路信号接到同一个Scope里。不要觉得这一步多余，这是你理解卡尔曼滤波价值最快的方式。

纯积分位移会对速度测量的每一次噪声都进行累积，即使噪声均值为零，随机游走效应也会让积分结果随着时间的推移越漂越远；更不用说轮速滑移那种系统偏差，会直接让积分线性发散。卡尔曼滤波之所以不飘，是因为它内部始终用模型去约束状态演化，测量只用来修正，而不是无条件接受。这种“约束”效果，在仿真图上非常直观。

5. 参数整定：Q和R怎么调才能又稳又准

5.1 设计一套能说明问题的仿真场景

参数整定不能上来就瞎调，设计一套合理的仿真输入很关键。我常用的场景是这样：0到5秒加速度恒定为2 m/s^2，车辆匀加速；5到10秒加速度为0，匀速行驶；10到15秒加速度为-3 m/s^2，减速；15到20秒再匀速。这个工况覆盖了加速、巡航、减速三个典型纵向工况，足够暴露出估计器在不同阶段的跟踪能力。

测量噪声方差按实际情况设，比如轮速计的测量噪声标准差大概是0.5 m/s，那 R 就取0.25。加速度噪声的标准差可能是0.3 m/s^2，那 q 可以取0.1左右。这些初始值不需要精确，能保证滤波器不崩就行，后面再细调。

仿真跑完之后，主要看三个指标：稳态时估计速度和真实速度的误差均值是否接近零；加减速过程中估计值能不能快速跟上，滞后有多大；估计位移和真实位移的偏差是否一直保持有界。只要前两个指标好，第三个一般不会差。

5.2 按“先粗调后细调”的顺序来

我的调参习惯是固定 R，先粗调 q。把 q 从0.001开始，按10倍梯度往上加，每跑一次看一眼跟踪效果。q 太小时滤波器几乎只信模型不信测量，你会发现估计曲线很平滑，但在加速度突变的时间点有明显的滞后，整个估计过程“钝钝的”。q 太大时滤波器几乎只信测量不信模型，估计曲线会跟着噪声剧烈抖动，看起来像直接拿传感器信号做低通滤波后的结果，失去了卡尔曼滤波的意义。

找到一个既不太钝又不太抖的中间值之后，再微调 R。R 调大的效果和调小 q 类似，区别在于 R 是传感器层面的参数，它只改变对测量的信任程度，不影响模型部分的动态。所以实际中我倾向于把 q 和 R 的比值作为主线来调，具体过程可以看下表：

还有一个经验：仿真中观察 P 的对角线元素。如果 p11 和 p22 收敛到稳定值后基本不变，说明滤波器进入了稳态；如果协方差持续增大或者忽大忽小，说明模型设置或者 Q 有问题。协方差是滤波器自己对自己的评价，它不能直接告诉你估计准不准，但能告诉你滤波器“内部状态是否健康”。

5.3 加速度输入有偏置怎么办

实际工程中IMU加速度零偏是最常见的坑。如果你在 Simulink 里给加速度 u 叠加一个0.2 m/s^2 的常值偏置，你会发现即使滤波器模型本身没问题，稳态时速度估计也会有一个固定偏差，而且位移估计会慢慢漂移。这不是卡尔曼滤波失效了，而是你的“模型输入”本身就错了。

这种情况下，有两条思路。第一条是把加速度零偏也扩进状态向量里，变成三阶状态：位移、速度、加速度偏置。这样滤波器会在线估计并修正零偏，代价是状态维数增加，可观测性分析要重新做。第二条是引入绝对位置观测，比如GPS，用位置观测量来锚定位移和速度，抑制由加速度偏置引起的漂移。两条路在实际系统中都有应用，我个人更喜欢第二条，因为GPS在现代车辆上几乎是标配，位置观测一进来，整个系统的长时间稳定性都会有质的提升。

6. 常见问题与排查技巧实录

6.1 协方差发散、估计结果飞掉的几种原因

我见过很多同学第一次跑通卡尔曼滤波，结果Scope里估计值直接飞到10的几十次方，第一反应是代码写错了。其实公式本身没问题的话，最常见的原因有两个。

一个是矩阵维度不匹配。MATLAB Function 模块里，A是2x2，P_pred是2x2，Q也是2x2，这些矩阵运算但凡有一个维度对不上，Simulink会报错，这个反而好发现。难发现的是维度匹配但含义搞错，比如把观测矩阵 H 写成行向量还是列向量搞混，导致 K 的维度变成1x2而不是2x1，结果算出来还能跑，但状态更新完全是乱的，估计值一会儿就飞掉了。这种情况我的排查办法是在模块里加几个 coder.extrinsic('disp') 临时打印中间变量，看 K 的维度是否正确。

另一个是初始协方差 P 和 R 设置不合理。如果 R 设得特别小，比如1e-6，而初始 P 又比较大，第一轮更新的 K 会接近1，滤波器几乎完全信任第一个测量值，状态估计会被一个带巨大噪声的初始测量直接带偏，后续想拉回来就很难。解决方法是给初始 P 一个合理的值，同时 R 不要设置得比传感器真实噪声小太多。

6.2 Simulink持久变量残留导致结果不对

用 persistent 变量做递推，有一个特别隐蔽的坑：Simulink 在“快速重启”模式或者多次连续运行同一模型时，persistent 变量不会自动清除，而是保留上一次仿真结束时的值。也就是说，第二次跑仿真的时候，滤波器的初值已经不是 [0;0] 了，而是上一次仿真最后一刻的状态。如果你没有意识到这一点，第二次的结果看起来就会和第一次完全不一样，甚至出现“这次跑和上次跑结果对不上”的现象。

解决办法有几个。最简单的是每次仿真前手动 Ctrl+D 重置一遍模型，或者在 MATLAB Function 模块内部做一个复位逻辑，比如额外增加一个 reset 输入，reset 为1时强制重新初始化 x 和 P。我在模型里一般都会留一个 reset 端口，方便批量跑参数扫描的时候不用反复清理持久变量。

6.3 测量更新频率和滤波器计算频率不一致

实际系统里轮速计可能50Hz，GPS只有10Hz，IMU是100Hz，多个传感器频率不一致是常态。但很多人的第一版Simulink模型是等间隔更新，把变步长仿真和滤波器固定步长混在一起，跑出来的结果时好时坏。

处理多速率问题，标准做法是把滤波器拆成两部分：预测部分每个计算周期都执行，测量更新部分只在有新测量到达时才执行。Simulink 里可以用函数调用子系统触发测量更新，或者简单一点，在 MATLAB Function 里加一个数据有效标志 valid，每次调用时判断 z 是否为有效值，无效就只做预测不做更新。这个实现方式不复杂，但对迁移到真实控制器非常重要，强烈建议在仿真阶段就把这个逻辑加进去，不然后面做嵌入式代码生成时还要大改。

6.4 MATLAB Function 模块相关的几个报错

Simulink 在编译 MATLAB Function 模块时经常报一些让人摸不着头脑的错误，我踩过的比较典型的有两种。

一种是“输出尺寸未定义”或者“输出大小不固定”。卡尔曼滤波函数的输出维度其实是恒定的，但如果你在函数里用了 if 分支返回不同维度的变量，Simulink 的静态分析就会报错。解决方法是把输出固定写成标量或者固定维度的数组，不要在分支里改变输出变量的尺寸。

另一种是常量折叠问题。dt 如果是从 Constant 模块输入的，Simulink 可能会把它当成可变信号，导致代码生成时矩阵维度分析失败。我的解决办法是在函数内部直接读一个全局参数，或者干脆把 dt 写成函数内部常量，反正仿真步长一般固定，没必要作为输入参数反复传。

6.5 滤波器发散：能观性检查比调参更重要

最后说一个很多人忽视的问题：卡尔曼滤波有一个隐含前提，就是系统必须是能观的。如果你的系统不可观，滤波器跑着跑着协方差就可能发散，或者估计结果对测量完全不响应。

以我们这个一阶运动学模型为例，如果观测只有速度，系统是能观的；如果观测只有位置，系统也是能观的，因为位置中包含了速度的信息；但如果两个观测都没有，反馈回路直接断开，滤波器就退化成了纯模型积分，任何模型误差都会无约束地积累。检查能观性，可以用MATLAB里的 ctrb、obsv 函数算一下能观性矩阵的秩，这比在仿真图上看曲线判断要靠谱得多。特别是当你修改了状态方程，新增了状态量之后，一定要重新做能观性分析，这是很多所谓“卡尔曼滤波调不出来”的问题的真正根源。

7. 从仿真到工程落地还可以怎么扩展

7.1 换用扩展卡尔曼滤波处理非线性模型

我前面用的是一阶运动学模型，它是线性的，卡尔曼滤波是最优估计。如果你希望把轮胎力、坡度阻力这些非线性动力学关系也纳进去，状态方程就会变成非线性的。这时候标准卡尔曼滤波就不好使了，需要换成扩展卡尔曼滤波。EKF的思路和标准KF完全一样，只不过用雅可比矩阵代替了原来的 A 和 H。Simulink里实现EKF也很方便，MATLAB Function模块里用符号求导或者手推雅可比矩阵都行，整体框架可以完全沿用这篇文章里的结构。

但我的建议是，能在线性模型框架里解决的，不要急着上EKF。EKF需要推导雅可比，需要更精细的噪声建模，参数整定难度也大一圈。很多实际工程问题，线性模型加合理的参数调度已经能解决80%的需求了。

7.2 联合仿真和代码生成

如果你的纵向车辆模型不是用积分器搭的，而是用CarSim或者CarSim和Simulink联合仿真搭的整车模型，那这篇文章里的滤波器模块可以不做任何修改直接接到CarSim输出的车速信号上，因为滤波器只关心测量输入 z 和控制输入 u，不关心上游模型是怎么实现的。

如果你最终是要把这个算法部署到实车上，那么在上位机验证完之后，可以用Embedded Coder把包含MATLAB Function模块的子系统生成C代码，然后集成到ECU里。我在这里要提醒的是，代码生成之前必须把数据类型的坑全部填掉：所有中间变量显式定义成double还是single，矩阵运算是用动态内存还是固定尺寸，这些都会影响生成代码的运行效率。把Simulink模型里通过验证的滤波器算法做定点化或单精度化，是量产前最重要的一步。

我自己做过一批这类项目之后，最大的体会是：卡尔曼滤波在仿真里跑通不难，难的是你真正理解每个参数、每个矩阵在物理上代表什么。如果你读完这篇文章能独立搭出第一版模型，再把Q和R调成符合自己传感器特性的数值，那你已经掌握了这类算法的骨架，以后无论是换车型、换传感器还是换更复杂的模型，都只是在这个骨架上填充细节的问题。