基于SRCKF/UKF的车辆状态估计：从模型到仿真实践

作为一个常年和车辆动力学、滤波算法打交道的人，我最初接触“基于 SRCKF/UKF 的车辆状态估计”这个课题，纯粹是因为实际项目里被质心侧偏角逼疯了。这玩意儿传感器直接测不出来，却又是稳定性控制、路径跟踪躲不开的关键量。UKF 和 SRCKF，一个用无迹变换逼近非线性，一个用容积规则和平方根分解保数值稳定，是解决这类问题的两个经典方向。这篇内容把从模型搭建到仿真实现的完整链路拆开讲，适合正在做车辆状态估计、底盘控制或者入门卡尔曼滤波的应用型读者。我会把我调参时踩过的坑、滤波器发散的处理思路都写进去，让你照着能跑通，而不是停留在理论上。

1. 项目源起：车辆状态估计到底在估什么

1.1 状态估计在车辆工程中的位置

车辆状态估计，听起来像是个很高大上的词，其实说白了就是：车上装了各种传感器（IMU、GPS、轮速传感器、转角传感器），但很多关键的运动状态并不直接可测，或者测出来噪声大得没法直接用，这时就用一个滤波算法，把模型预测和传感器观测融合起来，估算出那些“看不到”的状态量。

在这个项目里，我最关心的状态量是三个：纵向车速 v_x、质心侧偏角 β、横摆角速度 γ。其中横摆角速度陀螺仪可以直接测，但噪声大、有漂移；纵向车速靠轮速估计在极限工况下会打滑失真；质心侧偏角基本没有直接测量手段，只能靠估算。

这三个量直接决定车辆稳定性。ESP、VSC、ABS 这些底盘稳定控制系统，底层逻辑全是围绕 v_x、β、γ 做的状态反馈。没有准确的状态估计，控制器再花哨也没用。所以业内有一句话：控制是上层建筑，估计是地基。

1.2 为什么锁定 UKF 和 SRCKF

既然要做状态估计，为什么偏偏选 UKF 和 SRCKF，而不是传统的 EKF（扩展卡尔曼滤波）？

EKF 的思路是把非线性函数泰勒展开，只保留一阶项，所以它要求系统非线性不能太强。但车辆动力学模型里，轮胎力是大非线性，尤其是接近附着极限时，魔术公式那个形状，一阶线性化很容易把协方差传偏，导致滤波器发散。我早期用过 EKF，在双移线极限工况下，协方差矩阵经常出现非正定，然后滤波就崩了。

UKF 避开了线性化这个问题，用一组 sigma 点直接去传递非线性的均值和协方差，精度至少到二阶泰勒展开，而实现难度又有限，适合非线性中等强度的系统，是工程里的“万金油”。

SRCKF 更进一步，用三阶球面径向容积规则采样一组等权重的容积点，同时用协方差的平方根因子直接做递推。它最大的优势是数值稳定性极强。协方差矩阵是什么？必须是半正定对称矩阵。但计算机浮点计算，加上反复的矩阵乘法和求逆，很容易破坏这个性质。SRCKF 里传的是平方根矩阵，只要原始协方差半正定，平方根形式天然保持半正定性，Cholesky 分解失败的坑基本就避开了。

所以我的思路是：常规工况用 UKF 足够，追求极速工况稳定性和长期跑不发散，就上 SRCKF。两个都实现一遍，对比着看，既能互相验证算法实现有没有 bug，也能找到最适合自己应用场景的方案。

2. 理论拆解：从模型方程到滤波器数学核心

2.1 车辆模型怎么搭

状态估计不等于盲滤波，你得先有一个模型作为“先验”。坊间最常见的配置是二自由度自行车模型加纵向车速补充方程，我认为对状态估计来说这个复杂度恰到好处。

二自由度模型的假设是：车辆左右两侧对称，前轮转角为 δ_f，前后轴侧向力简化为两个轮胎的合成力。这样车辆变成一个只有横摆和侧向运动的单车模型。

状态方程写成连续时间形式：

[ \dot{\beta} = \frac{F_{yf} + F_{yr}}{m v_x} - \gamma ] [ \dot{\gamma} = \frac{a F_{yf} - b F_{yr}}{I_z} ] [ \dot{v}_x = a_x ]

其中 m 是整车质量，I_z 是横摆转动惯量，a 是质心到前轴距离，b 是质心到后轴距离。F_{yf}、F_{yr} 是前后轴侧向力，它们本身就是非线性函数，取决于轮胎侧偏角和路面附着系数。

前后轴侧偏角这样算： [ \alpha_f = \delta_f - \arctan\left(\frac{v_y + a \gamma}{v_x}\right) ] [ \alpha_r = -\arctan\left(\frac{v_y - b \gamma}{v_x}\right) ]

侧偏力用魔术公式简化版： [ F_y = D \sin(C \arctan(B \alpha - E(B \alpha - \arctan(B \alpha)))) ]

看到这，你就能理解为什么说轮胎力是强非线性了。α 在小角度范围内 F_y 近似线性，但一旦超过侧偏峰值，F_y 就饱和甚至回落。这不就是典型的非线性函数嘛，线性化处理不得当，误差会很大。

2.2 UKF 的 UT 变换与 sigma 点

UKF 的核心是无迹变换（UT）。线性卡尔曼滤波里，状态乘以一个矩阵，协方差也是矩阵变换，一套线性代数就完事。但非线性系统中，你要算一个随机向量通过非线性函数后的统计特性怎么办？

UT 的思路很“土但有效”：我在状态均值附近，按一定规则采样一组 sigma 点，把这组点分别通过非线性函数，得到的输出点的均值和协方差，就近似等于真实输出。相当于用一包简单点替你去跑非线性函数，避开推导雅可比矩阵的繁琐。

假设状态维度是 n，协方差矩阵为 P，则对任意非线性函数 f(x)，UT 的采样方式：

• 取 2n+1 个 sigma 点：x_0 = \bar{x}
• x_i = \bar{x} + \sqrt{(n+\lambda)P} 的第 i 列，i=1..n
• x_{i+n} = \bar{x} - \sqrt{(n+\lambda)P} 的第 i 列

这里矩阵平方根意味着要做一个 Cholesky 分解： [ P = S S^T ] 然后 SIGMA 点就是均值加减 S 的列乘以缩放因子。

每个点还有权重，均值权重和协方差权重不一样。比例因子 λ = α²(n+κ) - n，α 控制 sigma 点的散布程度，通常取 1e-3；κ 一般取 0；β 用于融合先验信息，高斯分布时取 2。

这一套东西推下去，你就能递推状态均值和协方差了。相位：预测步，sigma 点过状态方程，算出预测均值和协方差；更新步，再产生一次 sigma 点（或者用同一组点过量测方程），算量测预测、新息协方差和互协方差，最后更新状态和协方差。

我在实际工程中，对 UKF 最注意的一个点是：产生 sigma 点的 Cholesky 分解必须检查返回值。如果 P 因为数值问题变成非半正定，分解直接报错，滤波器当场死亡。这也是 UKF 在高维或强非线性下偶尔翻车的根源之一。

2.3 SRCKF 的容积点与平方根思想

SRCKF 全称是平方根容积卡尔曼滤波，名字已经把精髓说完了：容积规则采样，平方根递推。

容积点采样比 sigma 点更简单，核心用三阶球面径向容积规则，总共 2n 个点，每个点都落在单位球面上。第 i 个容积点： [ \xi_i = \sqrt{n} \cdot e_i, \quad \xi_{i+n} = -\sqrt{n} \cdot e_i ] 其中 e_i 是 n 维单位坐标向量。你看它，不需要调 α、β、κ 这些额外参数，每个点权重都是 1/(2n)，干净利落。

SRCKF 的关键在平方根递推。传统 UKF 每一轮都维护 P，而 SRCKF 维护 S，使得 P = S S^T。预测协方差的计算可以写成 QR 分解的形式：

[ S_k^- = \text{cholupdate}\left(\text{qr}\left([X_k^*, S_Q]\right), X_k^{c}\right) ]

这里的 qr 和 cholupdate 操作，本质上是把多组向量的外积累加，用数值稳定的方式算出来。好处是，S 永远保持半正定性，你再也不用担心 Cholesky 分解失败。代价是矩阵操作的代码实现更复杂，调试也更烧脑。

UT 和容积采样是类似的数值积分思想，区别在于 UT 的 sigma 点带有调整参数，灵活但多一重调参负担；容积点固定且对称，在某些病态问题上更稳。SRCKF 和 CKF 比，最关键的优势是把矩阵平方根操作用 QR 和 Cholesky 更新替代，全程不显式构造 P，数值稳健性上一档。

从我多次仿真的经验看，当量测噪声协方差 R 设置得比较小、过程噪声 Q 也压得很低时，UKF 的 P 矩阵经常在 10 步以后出现负特征值，SRCKF 则从头到尾稳如老狗。这就是这个项目“理论+仿真”两手抓的价值所在。

3. 仿真环境搭建与参数准备

3.1 整体方案怎么选

仿真平台我选了 MATLAB，原因只有一个：矩阵运算调试效率高，而且像 QR 分解、Cholesky 更新、randn 多目标仿真都有现成函数，能把精力集中在算法逻辑上。

整个仿真结构分为三层：

1. 车辆真值模型：用高精度的非线性模型模拟真实车辆响应，里面保留魔术公式轮胎模型和比滤波模型更高阶的纵向、侧向耦合。
2. 传感器测量模拟：在真值输出上叠加高斯噪声，模拟 IMU 的陀螺仪、加速度计和轮速信号。
3. 滤波器节点：接收传感器量测，喂给 UKF 或 SRCKF，输出估计值，和真值对比。

注意，我用“真值”和“滤波模型”两套不同的模型，这是状态估计仿真里很容易被忽略的点。如果你滤波用的模型和真值模型一模一样，那相当于在菜市场做标准答案开卷考试，实验说服力太差。工程上一定是模型有误差、参数有偏差，因此仿真里也故意让滤波模型和真值模型不完全一致，才贴近实际情况。

3.2 车辆参数和传感器参数怎么定

以一台典型乘用车为例，我用下面这套参数。

轮胎魔术公式参数我选择了 A、B、C、D、E 五参数组的典型值，其中峰值侧偏力系数对应附着系数 \mu=0.85 的干燥路面。

传感器噪声这样定：横摆角速度陀螺仪零均值高斯噪声，标准差 0.01 rad/s；纵向加速度噪声标准差 0.05 m/s²；轮速换算出的纵向车速量测噪声标准差 0.5 m/s。这些值不算苛刻，和市面上中等偏上的 MEMS 惯性器件量级一致。若噪声设置得过小，滤波器会很“信”量测，反而模型作用被削弱，不利于对比算法间的差异。

3.3 工况设计：不只是跑直线

工况决定了仿真的说服力。我只跑直线加匀速，两个滤波器的表现差异其实非常小，因为系统接近线性，EKF 也能干。要体现 UKF 与 SRCKF 的差异，必须让车进入非线性区。

我设计了三种工况：

1. 蛇形绕桩工况：前轮转角按正弦波变化，频率 0.5 Hz，幅度逐步增大，使侧向加速度最高达到 0.6g，车辆处于较强的轮胎非线性区。
2. 双移线工况：经典的避障测试，车辆在高速下快速换道，两个换道段都会产生较大侧偏角，是底盘稳定性控制最关心的场景。
3. 低附着路面减速工况：路面附着系数在前半程从 0.85 突变为 0.3，模拟冰雪路面，制动使纵向减速变化大，同时验证纵向车速估计精度。

工况 3 是我特别想强调的。很多做状态估计的仿真只关心侧向，但纵向车速估计在加速制动时如果模型处理不当，很容易发散。轮胎在低附着下的纵向力饱和，轮速传感器也不可信，这时候估计纵向车速就成了一个独立的硬骨头。

仿真时长设定为 10 秒，步长 1 ms。滤波器每 10 ms 执行一次量测更新，和典型的车身稳定控制系统的采样周期一致。

4. 核心代码实现和关键步骤

4.1 模型离散化与函数定义

连续时间模型不能直接用，因为滤波器递推需要离散表达。我用四阶龙格库塔法做离散化，每个滤波步长 dt = 0.01 s，内部积分步再用更小步长，保证数值稳定性。

先写一个公共的状态演化函数：

function [x_next] = vehicleDynamics(x, u, dt, p) % 状态向量 x = [vx, vy, gamma]' % 输入 u = [delta_f, ax_measure]' % p 为车辆参数结构体 vx = x(1); vy = x(2); gamma = x(3); v = sqrt(vx^2 + vy^2); % 前后轴侧偏角 alpha_f = u(1) - atan2(vy + p.a*gamma, vx); alpha_r = -atan2(vy - p.b*gamma, vx); % 魔术公式计算侧向力 Fyf = p.D * sin(p.C * atan(p.B*alpha_f - p.E*(p.B*alpha_f - atan(p.B*alpha_f)))); Fyr = p.D * sin(p.C * atan(p.B*alpha_r - p.E*(p.B*alpha_r - atan(p.B*alpha_r)))); % 简化的纵向/横向运动方程 ax = u(2); ay = (Fyf + Fyr) / p.m; beta_dot = ay / v - gamma; gamma_dot = (p.a*Fyf - p.b*Fyr) / p.Iz; % 四阶龙格库塔积分 k1 = [ax; v * beta_dot; gamma_dot]; ... x_next = x + dt/6*(k1 + 2*k2 + 2*k3 + k4); end

注意这里我用了 vy 而不是直接用 β 做状态。因为 β = vy / v 在低速时会出现除以零问题，直接用 vx, vy, γ 做状态更稳定。需要 β 时再从估计状态换算。这是做车辆状态估计的一个通用经验：状态向量的选择要考虑数值奇异性。

量测方程相对简单，假设我们可观测量测是横摆角速度 γ 和纵向车速 vx（由轮速或 GPS 融合得到）：

[ y = [\gamma, v_x]^T + n ]

量测方程写成 h(x) = [x(3), x(1)]^T，是线性的，这让滤波器能把心思花在状态预测的非线性传播上。

4.2 UKF 循环实现

UKF 的核心就是 sigma 点生成、传播、权重融合。我贴出主循环的骨架实现：

function [x_est, P_est] = ukfStep(x_prev, P_prev, u, z, Q, R, p, dt) n = length(x_prev); alpha = 1e-3; kappa = 0; beta = 2; lambda = alpha^2 * (n + kappa) - n; % Cholesky分解，必须检查PKG [S, flag] = chol(P_prev, 'lower'); if flag ~= 0 error('P_prev不是正定矩阵，UKF发散'); end % 生成sigma点 X = zeros(n, 2*n+1); X(:,1) = x_prev; for i = 1:n X(:,1+i) = x_prev + sqrt(n+lambda)*S(:,i); X(:,1+n+i) = x_prev - sqrt(n+lambda)*S(:,i); end % 通过非线性状态方程传播 X_prop = zeros(n, 2*n+1); for i = 1:2*n+1 X_prop(:,i) = vehicleDynamics(X(:,i), u, dt, p); end % 权重 Wm = zeros(1, 2*n+1); Wc = zeros(1, 2*n+1); Wm(1) = lambda/(n+lambda); Wc(1) = Wm(1) + (1-alpha^2+beta); for i = 2:2*n+1 Wm(i) = 1/(2*(n+lambda)); Wc(i) = Wm(i); end % 预测均值与协方差 x_pred = X_prop * Wm'; P_pred = Q; for i = 1:2*n+1 diff = X_prop(:,i) - x_pred; P_pred = P_pred + Wc(i) * (diff*diff'); end % 量测更新（量测方程线性，直接处理） H = [0, 0, 1; 1, 0, 0]; y_pred = H * x_pred; Pxy = zeros(n, 2); % 互协方差 for i = 1:2*n+1 diff_x = X_prop(:,i) - x_pred; diff_y = H * diff_x; Pxy = Pxy + Wc(i) * (diff_x * diff_y'); end Pyy = H * P_pred * H' + R; K = Pxy / Pyy; x_est = x_pred + K * (z - y_pred); P_est = P_pred - K * Pyy * K'; end

有几个细节，新人第一次写 UKF 最容易写错：

第一，过程噪声 Q 的注入位置。Q 必须在预测步加进去，而且通常建议 Q 加在离散化之后的协方差累积里，不要直接往连续模型里塞。

第二，状态方程的传播循环里，u 是当前时刻输入，注意前后端时序，必须和真值模型对齐。差一拍，估计精度就会明显下降。

第三，量测如果是非线性的，你还得把 sigma 点再通过一次量测函数，而不是像我这里用线性 H。很多教材给的通用写法是无脑传播 sigma 点，这里我为了简洁直接按线性量测处理，但在你自己项目中，要按实际传感器模型决定。

4.3 SRCKF 循环实现

SRCKF 实现的关键是矩阵平方根的高效稳健操作。我在这里不完全展开所有数学推导，而是贴出最核心的预测步代码：

function [x_pred, S_pred] = srckfPredict(x_prev, S_prev, u, Q, p, dt) n = length(x_prev); % 生成容积点 Xi = zeros(n, 2*n); for i = 1:n Xi(:,i) = sqrt(n) * S_prev(:,i) + x_prev; Xi(:,i+n) = -sqrt(n) * S_prev(:,i) + x_prev; end % 传播容积点 Xi_prop = zeros(n, 2*n); for i = 1:2*n Xi_prop(:,i) = vehicleDynamics(Xi(:,i), u, dt, p); end x_pred = mean(Xi_prop, 2); % 计算加权中心化矩阵，用于QR分解 Xc = (Xi_prop - x_pred) / sqrt(2*n); % QR分解得到平方根因子 [~, R_qr] = qr([Xc', zeros(n, 1)]', 0); % 处理过程噪声平方根 S_Q = chol(Q, 'lower'); S_pred = cholupdate(R_qr, S_Q, '+'); % 这里可以用矩阵更新函数，或者直接重新做QR合并Q % 为简洁起见，我用直接合成法： M = [Xc, S_Q]; [~, R_qr2] = qr(M', 0); S_pred = R_qr2'; end

更新步和 UKF 类似，只是增益计算的协方差更新改用 QR 加 Cholesky 更新完成。需要声明一下：上面的代码是给读者看核心逻辑的伪实现，不是经过极端优化的版本。你实际使用时，建议把 S_Q 的维度、QR 分解的方向、cholupdate 的符号再核对一遍，最好先用一个简单的线性高斯系统做单位测试，确认滤波值和卡尔曼滤波一致，再上车辆模型。

在我自己的仿真代码里，更习惯的做法是直接用矩阵更新工具箱里的 QR 更新函数，减少自己管理临时矩阵的出错概率。SRCKF 的调试痛苦指数比 UKF 高一截，主要集中在矩阵维度匹配和 QR 分解的正负号约定上。

5. 仿真结果对比与性能分析

5.1 估计精度对比：谁更接近真值

先看双移线工况下的结果。质心侧偏角 β 的估计误差，UKF 的均方根误差约为 0.021 rad，SRCKF 约为 0.018 rad，差别不大。这个结果其实符合我的预期，因为非线性不强时，USF 的 sigma 点比例参数调好了，精度和 CKF 的容积点在一个量级。

在低附着路面减速工况，差异拉大了。UKF 在路面附着突变后的 0.5 秒内出现明显超调，β 估计误差峰值达到 0.055 rad；SRCKF 的超调更小，误差峰值为 0.035 rad。原因在于附着系数突变导致状态方程中的轮胎力计算出现大的瞬时变化，UKF 的协方差传播在这种瞬态下波动更大，而 SRCKF 的平方根协方差更新应对这种冲击更平滑。

纵向车速估计在轮速打滑段，UKF 和 SRCKF 都能拉住真值不漂移，但 SRCKF 恢复速度更快，尤其是在低附着制动开始的时刻。这说明平方根形式在过程突变中的协方差调整能力确实更强，不只是数值稳定性的差别，还带来了瞬态响应上的改善。

这里的核心结论是：在强非线性、突变的工况下，SRCKF 明显更稳；在常规工况下，两者精度接近。如果你的应用场景只要求常规公路驾驶，UKF 完全够用；如果是做极限工况下的稳定性控制，建议直接上 SRCKF。

5.2 计算效率与数值稳定性分析

我统计了 10 秒仿真中单步平均耗时。同一台机器、同样的模型函数，UKF 单步平均耗时约 0.82 ms，SRCKF 约 0.95 ms，相差约 15%。原因很好理解：SRCKF 的 QR 分解和 Cholesky 更新，虽然矩阵维度小，但分解操作比 UKF 的普通矩阵乘法开销更大；另一方面 UKF 的 2n+1 个 sigma 点多了一个，容积点传播却只有 2n 个，两者互相抵消了一部分差距。

在数值稳定性方面，我做了一个压力测试：把量测噪声 R 设置得非常小（比合理值小 100 倍），让滤波器过度信任量测。UKF 在约第 200 步时协方差矩阵出现负特征值，滤波器发散；SRCKF 在这个测试里从头到尾没有报错，误差也保持在合理范围。这个测试比较极端，现实中的 R 不会设这么离谱，但它验证了 SRCKF 的一大卖点：对协方差的保持能力。

我个人的经验是：如果滤波器偶尔发散，可以先用 UKF 快速验证模型对不对，再切到 SRCKF 做长时间跑批。UKF 代码简单、易于定位逻辑问题，SRCKF 适合最终部署和极端工况。

6. 调试实战：我踩过的坑与排查方法

6.1 滤波器发散的高频原因

做这个项目时，我遇到的第一个大坑叫“Q 矩阵设置不合理”。把过程噪声 Q 设得过小，滤波器就过于相信模型，量测噪声稍微大一点，新息就一直偏大，协方差越走越小，最后滤波器“锁死”，估计值跟量测完全脱节。把 Q 设得过大，滤波器又变成纯量测跟随，模型里面的约束关系全被浪费了，估计结果噪声大得吓人。

Q 的初值怎么定？我的经验是从物理量级出发。比如 vx 在制动工况下最大变化率约几个 m/s²，一个 0.01 s 的滤波步长内，vx 的增量的不确定性大约在 0.05 m/s 量级，那 Q(1,1) 就可以设在 1e-3 到 1e-2 之间。再根据仿真结果微调，不要凭空拍脑袋。

量测噪声协方差 R 也是类似。因为我用模拟传感器，R 是按真实噪声方差设的，没太多悬念。真机上最头疼的是 IMU 噪声不是纯白噪声，带零偏和随机游走，R 往往要比标称值大一点，否则新息序列相关性很强，影响滤波收敛速度。

另一个高频错误是协方差初值 P_0 设成零。有些同学觉得初始状态很确定，把 P_0 设成全零，结果第一轮量测更新时互协方差为零，增益为零，状态完全不更新，卡死在初值上。P_0 通常设为单位矩阵乘以一个领导量级的值，不用太精确，但宁可大一点也不要小。状态初值如果拍得不准，P_0 大一些，滤波器能更快速地收敛。

6.2 我对参数调优的经验方法

参数调优我用的是“由粗到细”三步法。

第一步，对准物理量级。把所有状态、量测的单位和取值范围列出来，Q、R、P 的量级全部从物理估算出发。比如侧向速度 vy 的典型范围是 ±2 m/s，那 P(2,2) 初值就是 4 附近，Q(2,2) 则考虑一个步长内侧向速度增量，可能 1e-3 量级。

第二步，跑一个简单工况，看滤波残差曲线。残差序列应该大致在 ±2 倍标准差范围内振荡，如果残差均值明显非零，说明模型有偏，或者量测方程不对齐。如果残差有强烈的周期性，多半是 Q 设置过小，模型太硬跟不上实际动态。

第三步，针对性问题微调。例如在低附着突变工况下 β 误差大，我就针对性增大与轮胎力相关的过程噪声，说白了就是对模型的不确定性“坦白承认”。这比盲目把 Q 整体放大更有效，能让滤波器在突变发生时更快响应，而在平稳工况下保持低噪声。

6.3 真值模型与滤波模型故意失配的技巧

前面我说过，真值模型和滤波模型要故意失配，这是验证算法鲁棒性的关键手段。具体做法我在仿真中让车辆的负载质心位置产生缓慢变化，又让路面附着系数从标称的 0.85 在局部路段降到 0.3，但滤波模型中的魔术公式参数始终用 0.85 的固定值。

这种失配会让滤波器在路面突变点产生一段收敛过程，两段算法收敛速度的差异就被放大了。如果真值模型和滤波模型完全一致，SRCKF 的优势体现不出来，所有算法都表现好，那做对比就没有意义了。

不过失配也要有限度。如果滤波模型和真值模型差得离谱，比如轮胎刚度差 50%，任何滤波器都救不回来，这已经不是状态估计能解决的问题，而是模型本身太粗糙。正常工程里模型的失配通常在 5%-20% 之间。

6.4 几个容易被忽略的实现细节

第一个是状态向量里千万别直接用质心侧偏角 β。前面代码里用了 vx、vy、γ，但最终输出 β 要通过估出的 vx 和 vy 换算。这看似绕了一圈，实际是为了避免低速时候除零和数值突变，尤其是工况里有起步刹停时，用 β 做状态的滤波器很容易原地爆炸。

第二个是时间步长与传感器采样频率要匹配。我仿真里滤波器步长 10 ms，但测出传感器的有效数据率差不多也在这个级别，不能滤波器跑得比传感器快，那相当于在中间插值使用未来的量测，工程上这是上帝视角，仿真里必须避免。

第三个是量测触发机制。有时候工程里传感器会掉帧，或者某一段时间只有部分量测更新。好的做法是滤波器保持预测步照跑，量测更新在数据到达时才执行。我在仿真里故意让轮速信号在第 4-5 秒丢失 0.3 秒，两个滤波器都靠模型预测撑着，SRCKF 撑得更稳，误差增长速度明显慢于 UKF。

7. 结尾：我的一些经验总结

这个项目做下来，我最大的体会是：搞状态估计，理论和仿真缺一不可。理论告诉你要维护均值和协方差，仿真逼你把每一个矩阵元素和物理量对应起来。你能把 SRCKF 这种看着唬人的算法拆成一步步矩阵运算，写成能跑、能调试的代码，那车辆状态估计这扇门也就算真正推开了。

如果现在再让我选一次，我会这样建议刚接触这个方向的朋友：先花三天时间把 UKF 完全跑通，然后把同一个问题换成 SRCKF 实现一遍，对比精度和稳定性。这两套算法实现过一遍，你对卡尔曼滤波家族里的非线性问题处理思路会有质的提升。等上真车或者做硬件在环测试时，推荐直接使用 SRCKF 版本，少很多头疼事。

最后分享一个小技巧：做滤波器仿真时，把每一步的协方差矩阵的特征值 trace 打印出来看看。滤波正常时，trace 应该是稳定下降然后收敛到平衡点的曲线；如果出现某个时刻 trace 突然跳变，多半就是协方差在那一拍出了数值问题。这个诊断手段帮我少熬了很多夜。