尧图网页设计 
车还没拐弯估计的位置已经飘到隔壁车道去了——这是我第一次用线性卡尔曼滤波做车辆状态估计时遇到的真实情况。后来把模型换成CTRV恒定转率和速度运动学模型线性卡尔曼直接失效因为状态转移方程里的正弦余弦项没办法用线性矩阵表达。那之后我花了很长时间把扩展卡尔曼滤波EKF和无迹卡尔曼滤波UKF都完整实现了一遍在Matlab和Simulink环境里搭了一套可复用的车辆状态估计框架。这篇内容算是把整个过程的思路、公式推导、代码细节和踩坑记录都整理出来给正在做类似工作的朋友一个参考。这个方向在工程上的典型应用场景包括自动驾驶中的车辆定位与轨迹预测、车辆动力学参数实时辨识、车载传感器GPS、IMU、轮速传感器的信号融合、车队控制中的前车状态估计等。无论你在做哪个方向只要涉及根据带噪声的传感器数据实时推算车辆的真实运动状态EKF/UKF就是绕不开的两种经典方案。本文会用Matlab代码和Simulink模型两种形式展示完整实现并重点说明为什么在某些场景下UKF比EKF更稳以及噪声协方差矩阵到底该怎么调。1. 为什么线性卡尔曼滤波处理不了车辆运动估计先看一个非常典型的场景车辆在平面上运动状态量取为位置x、位置y、速度大小v、航向角ψ和航向角变化率ψ_dot这就是CTRV模型。它的连续时间状态方程是dx/dt v * cos(ψ) dy/dt v * sin(ψ) dv/dt 0 假设加速度为零实际通过过程噪声吸收 dψ/dt ψ_dot dψ_dot/dt 0如果系统是线性的状态转移方程可以写成 x_k A * x_{k-1} B * u_k w_k然后直接用标准卡尔曼滤波的预测和更新两步就能搞定。但你看这个模型里的 cos(ψ) 和 sin(ψ)它们是状态 ψ 的非线性函数。写成离散形式后状态转移本身就没有办法用一个常矩阵 A 来表示。整个系统是非线性的标准卡尔曼滤波在这个模型下根本不适用。有的朋友可能会想那我用恒速度CV模型把状态量改成 x方向速度vx和y方向速度vy不就没有三角函数了吗。确实可以但这样等于放弃了航向角这个关键状态量。在车辆运动估计中航向角恰恰是最重要的状态之一无论是做轨迹预测还是做传感器融合你都需要知道车头的朝向角。而且CV模型的线性化假设太强稍微遇到一个弯道位置预测就会出现明显偏移。非线性的另一个来源是量测方程。你用GPS观测车辆位置观测值一般是经纬度转换后的平面坐标这个观测方程还算简单是线性的。但如果你用雷达或毫米波雷达观测量测往往是目标相对于雷达的距离和方位角。距离方程是 sqrt((px - rx)^2 (py - ry)^2)方位角是 atan2(py - ry, px - rx)这两个都是状态量的非线性函数。就算状态转移可以勉强近似成线性量测方程也会引入非线性。非线性系统的最优状态估计在数学上没有一个闭式解——也就是说你没有办法像线性卡尔曼那样用一组简洁的矩阵递推公式精确算出后验分布。EKF和UKF本质上都是一种次优近似它们的核心思路都是把非线性问题变成可以算的问题。EKF的做法是对非线性函数做一阶泰勒展开用雅可比矩阵来近似UKF的做法是用一组采样点Sigma点直接进行非线性变换然后从变换后的点中恢复均值和协方差。两种思路各有优劣后面章节会展开讲。2. 车辆模型的取舍状态向量、过程方程与量测方程的设计逻辑做状态估计的第一步永远不是写代码而是先确定状态向量里面有什么过程模型怎么走量测模型是什么。这三者直接决定滤波器的结构和最终效果。2.1 状态向量怎么选从CTRV模型说起我这次采用的是CTRV模型状态向量是x [px; py; v; ψ; ψ_dot]其中 px、py 是车辆在全局坐标系下的平面位置v 是纵向车速ψ 是航向角ψ_dot 是航向角变化率也就是横摆角速度。为什么选这五个量因为它们在车辆运动学中构成了一个自洽的闭环位置的变化率由速度和航向角决定航向角的变化率由 ψ_dot 决定。系统输入中没有加速度项而是通过过程噪声来吸收车辆的加速和减速这是CTRV的标准做法。相比直接加入加速度项的constant accelerationCA模型CTRV在转弯场景下有更好的适应性因为ψ_dot是显式的状态量模型天然支持圆弧运动轨迹。过程方程的离散形式为假设采样间隔为 Δtpx_{k1} px_k (v_k / ψ_dot_k) * (sin(ψ_k ψ_dot_k * Δt) - sin(ψ_k)) 当 |ψ_dot_k| eps px_{k1} px_k v_k * cos(ψ_k) * Δt 当 |ψ_dot_k| ≈ 0py_{k1} py_k (v_k / ψ_dot_k) * (-cos(ψ_k ψ_dot_k * Δt) cos(ψ_k)) 当 |ψ_dot_k| eps py_{k1} py_k v_k * sin(ψ_k) * Δt 当 |ψ_dot_k| ≈ 0v_{k1} v_k ψ_{k1} ψ_k ψ_dot_k * Δt ψ_dot_{k1} ψ_dot_k这里有个重要的工程细节当 ψ_dot 趋近于零时包含除法的公式会出现数值问题所以必须做一次条件判断退化成直线运动模型。这个细节在实际代码里非常关键很多新手第一次跑CTRV模型发散就是没处理这个边界条件。我建议在代码里先做一个阈值判断比如 |ψ_dot| 1e-6 就走直线模型分支。2.2 过程噪声的建模Q矩阵不是拍脑袋定的过程噪声协方差矩阵 Q 反映的是模型本身的误差。CTRV模型的误差来源主要有两个纵向加速度和横摆角速度的角加速度。也就是说模型认为 v 和 ψ_dot 是常量但实际上车辆会加速、会转向这个模型误差需要通过过程噪声来覆盖。通常做法是设定两个噪声参数直线加速度噪声标准差 σ_a 和角加速度噪声标准差 σ_ω。把连续时间的加速度噪声映射到状态空间得到过程噪声协方差矩阵。由于过程方程是v_k和ψ_dot_k的非线性函数严格的离散化处理需要计算噪声对各个状态的线性化传递矩阵。工程上常用的简化方式是GPTGeneralized Pseudo-Transformation近似——通过直接对状态转移方程求偏导推得Q矩阵的具体形式。对CTRV模型常见的Q矩阵形式是G [0.5 * Δt^2 * cos(ψ) , 0 0.5 * Δt^2 * sin(ψ) , 0 Δt , 0 0 , 0.5 * Δt^2 0 , Δt]Q G * diag([σ_a^2, σ_ω^2]) * G这个矩阵里每一项都有物理意义位置项的噪声与时间平方成正比因为加速度误差对位置的影响是二次积分速度项与时间一次方成正比横摆角速度同理。说实话这个Q矩阵是最难调的参数之一因为σ_a和σ_ω本身没有一个精确的物理测定方法更多是靠经验结合传感器特性来整定。后面的调参章节会专门讲怎么调。2.3 量测方程的设计GPS与IMU的量测特性差异量测模型取决于你手上的传感器。我做的系统里有两类量测可用GPS位置量测直接观测 px 和 py。量测方程是线性的z [px; py] R_gpsR_gps 是GPS的量测噪声协方差它的量级取决于GPS的精度。普通民用GPS精度在2~5米左右差分GPSRTK可以到厘米级。如果你需要在自动驾驶级别的精度RTK基本是标配但这不在本文讨论范围内日常实验用普通GPS数据看趋势完全够用。IMU量测IMU可以提供加速度和角速度。在本文的CTRV模型中我们主要用IMU提供的横摆角速度 ψ_dot_imu 和纵向加速度数据。如果直接把 ψ_dot_imu 当作量测量测方程就是z_imu ψ_dot R_imu也就是说IMU的观测值直接与状态向量中的ψ_dot对应。有些实现方案会把IMU的加速度作为控制输入而不是量测两种方案都可行。我的做法是把它作为量测来处理这样可以利用量测更新来修正对ψ_dot的估计而不是仅仅把它当作输入然后靠过程噪声去匹配。还有一个在车辆导航中常见的量测是轮速传感器提供的车速 v_wheel。同样地我们可以把它直接当作v状态量的量测。在设计量测方程时要特别注意各传感器的坐标系是否一致。GPS给的是全局坐标系ENU东北天下的坐标IMU给的是车体坐标系下的数据轮速传感器给的是车体纵向速度。如果你不加处理直接把IMU的加速度作为状态量对应的量测大概率会出问题。工程做法是对IMU数据做坐标变换把车体坐标系的角速度映射到导航坐标系对轮速数据则直接使用因为它本身就是纵向速度。这个坐标系问题在后面的Simulink实现部分还会再次出现这是一个非常容易被忽略的坑。3. EKF实现拆解从雅可比矩阵到状态递推的完整推导EKF扩展卡尔曼滤波是处理非线性系统最经典的方法思路简单粗暴把非线性函数在状态估计点附近做一阶泰勒展开然后套用标准卡尔曼滤波框架。因为只保留了一阶项所以EKF的精度取决于非线性函数的非线性程度。车辆运动模型中的三角函数虽然是非线性的但在小采样间隔下一阶展开的误差通常可控这也是EKF至今仍然大量使用的原因。3.1 EKF的预测与更新公式EKF的两步结构与标准卡尔曼滤波器完全一致只是把原有的矩阵乘法换成了非线性函数的求值加雅可比矩阵的传递预测步x_pred f(x_est) P_pred F * P_est * F Q更新步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这里的 F 是状态转移函数 f 对状态向量 x 的雅可比矩阵H 是量测函数 h 对状态向量 x 的雅可比矩阵。一切都围绕这两个矩阵展开。3.2 状态转移雅可比矩阵 F 的推导注意一个关键点雅可比矩阵是在当前预测点求值的。也就是说虽然非线性函数形式不变但矩阵的数值每个滤波周期都在变化必须每步重新计算。你在写代码时不能把F当成固定常数矩阵。对CTRV模型雅可比矩阵F5x5各元素是对状态向量各分量的偏导数。设:a (v / ψ_dot) * (sin(ψ ψ_dot * Δt) - sin(ψ)) b (v / ψ_dot) * (-cos(ψ ψ_dot * Δt) cos(ψ))即 px_{k1} px apy_{k1} py b。可以得到∂px_{k1}/∂px 1 ∂px_{k1}/∂py 0 ∂px_{k1}/∂v (sin(ψ ψ_dotΔt) - sin(ψ)) / ψ_dot ∂px_{k1}/∂ψ (v/ψ_dot) * (cos(ψ ψ_dotΔt) - cos(ψ)) ∂px_{k1}/∂ψ_dot -(v/ψ_dot^2) * (sin(ψ ψ_dotΔt) - sin(ψ)) (v/ψ_dot) * cos(ψ ψ_dotΔt) * Δt∂py_{k1}/∂px 0 ∂py_{k1}/∂py 1 ∂py_{k1}/∂v (-cos(ψ ψ_dotΔt) cos(ψ)) / ψ_dot ∂py_{k1}/∂ψ (v/ψ_dot) * (sin(ψ ψ_dotΔt) - sin(ψ)) ∂py_{k1}/∂ψ_dot (v/ψ_dot^2) * (cos(ψ ψ_dotΔt) - cos(ψ)) (v/ψ_dot) * sin(ψ ψ_dotΔt) * Δt∂v_{k1}/∂v 1其余为0 ∂ψ_{k1}/∂ψ 1∂ψ_{k1}/∂ψ_dot Δt ∂ψ_dot_{k1}/∂ψ_dot 1这个形式比较长我再次提醒一个关键点以上是ψ_dot ≠ 0时的形式。当|ψ_dot|极小时有些分母会变成无穷大必须在代码里做分支处理退化成直线运动模型后重新求偏导。直线模型的雅可比就是标准CV模型的形式简单很多。如果你偷懒不处理这个细节仿真中车辆一旦直线行驶数值就会异常。实际编码时我更建议用MATLAB的Symbolic Math Toolbox一次性导出符号雅可比矩阵然后再生成数值函数。当年我手推这个矩阵推错了好几次用了符号工具箱之后效率高了很多。可以这么写% 使用符号计算导出雅可比矩阵 syms px py v psi psi_dot dt % 定义状态转移函数 f [px (v/psi_dot)*(sin(psi psi_dot*dt) - sin(psi)); py (v/psi_dot)*(-cos(psi psi_dot*dt) cos(psi)); v; psi psi_dot*dt; psi_dot]; % 计算雅可比矩阵 F_sym jacobian(f, [px, py, v, psi, psi_dot]); % 导出为MATLAB函数 matlabFunction(F_sym, File, compute_F_matrix, Vars, {px, py, v, psi, psi_dot, dt});这样生成的 compute_F_matrix 函数可以直接被EKF的代码调用不会符号推导错误。3.3 量测雅可比矩阵 H 与滤波器代码实现对于GPS位置量测量测方程为h1(x) px h2(x) py所以H矩阵为H [1 0 0 0 0 0 1 0 0 0]如果加入IMU横摆角速度量测就把h3(x) ψ_dot 加进去对应H的第3行为 [0 0 0 0 1]。如果加入轮速量测h4(x) v对应H的第4行为 [0 0 1 0 0]。这个H矩阵是线性的不需要求偏导但要注意量测维度和噪声协方差矩阵R的维度要保持一致。以下是完整的EKF算法代码我已经在Matlab中验证过function [x_est, P_est] ekf_predict_update(x_est, P_est, z, dt, Q, R, use_gps, use_imu, use_wheel) % EKF预测步 psi_dot x_est(5); v x_est(4); psi x_est(3); % 根据psi_dot选择模型 if abs(psi_dot) 1e-6 % 直线运动模型 F [1 0 dt*cos(psi) -v*dt*sin(psi) 0; 0 1 dt*sin(psi) v*dt*cos(psi) 0; 0 0 1 0 0; 0 0 0 1 dt; 0 0 0 0 1]; x_pred x_est [v*cos(psi)*dt; v*sin(psi)*dt; 0; 0; 0]; else % 圆弧运动模型 F compute_F_matrix(x_est(1), x_est(2), v, psi, psi_dot, dt); dp v/psi_dot; x_pred [x_est(1) dp*(sin(psi psi_dot*dt) - sin(psi)); x_est(2) dp*(-cos(psi psi_dot*dt) cos(psi)); v; psi psi_dot*dt; psi_dot]; end P_pred F * P_est * F Q; % 构造量测向量和量测矩阵 H []; z_meas []; R_meas []; if use_gps H [H; 1 0 0 0 0; 0 1 0 0 0]; z_meas [z_meas; z(1); z(2)]; R_meas blkdiag(R_meas, R(1:2, 1:2)); end if use_imu H [H; 0 0 0 0 1]; z_meas [z_meas; z(3)]; R_meas blkdiag(R_meas, R(3, 3)); end if use_wheel H [H; 0 0 1 0 0]; z_meas [z_meas; z(4)]; R_meas blkdiag(R_meas, R(4, 4)); end % 更新步 S H * P_pred * H R_meas; K P_pred * H / S; x_est x_pred K * (z_meas - H * x_pred); P_est (eye(5) - K * H) * P_pred; end这个函数每次需要传入量测向量z并根据开关值决定使用哪些传感器。这种设计的好处是后续做传感器故障注入或效果对比时非常方便你不需要改写核心滤波器只要切换开关即可。3.4 EKF的局限性一阶展开带来的精度瓶颈EKF最大的问题在于一阶泰勒展开的近似能力有限。如果非线性的强度大比如ψ_dot很大、航向角快速变化时一阶近似会让均值和协方差的传播出现偏差。另一个问题是雅可比矩阵的计算容易出错尤其是比较复杂的模型手推偏导数非常容易漏项或算错符号。EKF还有一个隐含假设噪声必须服从高斯分布且经过非线性变换后仍近似高斯。如果系统中存在强非高斯噪声EKF的表现会明显下降。对车辆运动估计来说GPS的噪声在有遮挡物时会出现重尾特性此时EKF的性能会受到明显影响这也是后面要引入UKF的现实动机之一。4. UKF实现拆解Sigma点怎么选、权重怎么分、更新怎么做UKF和无迹变换Unscented Transform的设计哲学和EKF完全不同。它不计算雅可比矩阵而是找一组精心选择的采样点Sigma点让这些点经过非线性函数传播后用加权统计的方式恢复出均值和协方差。这样做的好处是不需要解析求导而且对非线性函数的近似精度能到二阶甚至更高因为Sigma点传播保留了更多非线性信息。4.1 无迹变换的原理无迹变换的核心思想是对n维高斯分布选取2n1个Sigma点这些点的均值和协方差与原始分布完全一致。将这些点分别通过非线性函数传播后得到的点集能够反映非线性变换后分布的均值和协方差特征。你可以把它理解为用一群点的分布去近似一个分布的统计特性而不是去近似非线性函数本身。Sigma点的生成公式为χ(0) x_mean χ(i) x_mean sqrt((n λ) * P) 的第i列i 1..n χ(in) x_mean - sqrt((n λ) * P) 的第i列i 1..n其中 λ α^2 * (n κ) - nα控制Sigma点相对于均值的扩散程度一般取一个较小的正值如0.001到1之间κ是次级缩放参数在高斯分布下通常取0或3-n。权重为Wm(0) λ / (n λ) Wc(0) λ / (n λ) (1 - α^2 β) Wm(i) Wc(i) 1 / (2 * (n λ))i 1..2nβ一般取2对应高斯分布下的最优值。每次计算sqrt((n λ) * P)时必须用Cholesky分解或矩阵平方根且要求P是正定矩阵。这个正定要求在实际仿真中很关键因为协方差矩阵经过多次递推后可能因为数值误差失去正定性。如果你在跑UKF时出现Matrix must be positive definite的报错大概率就是协方差矩阵被迭代坏了需要在每个周期做一次对称正定修复或使用平方根UKF。4.2 Sigma点生成与权重计算代码以下是在Matlab中实现Sigma点生成的完整代码function [Xi, Wm, Wc] generate_sigma_points(x, P, alpha, beta, kappa) n length(x); lambda alpha^2 * (n kappa) - n; % 计算矩阵平方根 sqrt_matrix chol((n lambda) * P, lower); % 生成Sigma点 Xi zeros(n, 2*n1); Xi(:, 1) x; for i 1:n Xi(:, i1) x sqrt_matrix(:, i); Xi(:, i1n) x - sqrt_matrix(:, i); end % 权重 Wm zeros(2*n1, 1); Wc zeros(2*n1, 1); Wm(1) lambda / (n lambda); Wc(1) lambda / (n lambda) (1 - alpha^2 beta); for i 2:(2*n1) Wm(i) 1 / (2 * (n lambda)); Wc(i) 1 / (2 * (n lambda)); end end这里用 chol 函数做Cholesky分解代替了sqrtm因为chol的计算效率更高且对正定矩阵更稳定。如果遇到chol分解失败的情况建议先对P做对称化处理 P (P P)/2再尝试分解。如果仍然失败可能是过程噪声Q设计过小导致协方差收敛到奇异需要检查Q的取值。4.3 Sigma点传播与量测更新UKF的预测和更新都围绕Sigma点展开。预测步将Sigma点逐一通过状态转移函数传播然后加权计算预测均值和协方差function [x_pred, P_pred] ukf_predict(Xi, Wm, Wc, dt, Q) n size(Xi, 1); num_sigma size(Xi, 2); % 传播Sigma点 Xi_pred zeros(n, num_sigma); for i 1:num_sigma Xi_pred(:, i) ctrv_process_model(Xi(:, i), dt); end % 加权计算预测均值 x_pred zeros(n, 1); for i 1:num_sigma x_pred x_pred Wm(i) * Xi_pred(:, i); end % 加权计算预测协方差 P_pred Q; for i 1:num_sigma diff Xi_pred(:, i) - x_pred; P_pred P_pred Wc(i) * (diff * diff); end endctrv_process_model 函数就是前文提到的CTRV离散状态转移函数这里不再重复实现但注意它同样要处理ψ_dot趋近于零的分支情况。UKF和EKF共用同一个过程模型函数这也是UKF的一个工程优势——你不需要额外推导雅可比矩阵。量测更新与预测是类似的过程先用量测函数传播Sigma点然后计算量测预测均值、新息协方差和互协方差最后计算卡尔曼增益并更新状态和协方差function [x_est, P_est] ukf_update(x_pred, P_pred, Xi_pred, Wm, Wc, z, R, use_gps, use_imu, use_wheel) n length(x_pred); num_sigma size(Xi_pred, 2); % 构建量测函数输出 % 假设量测包含 [px; py; psi_dot; v] Z zeros(4, num_sigma); H_rows 0; if use_gps, H_rows H_rows 2; end if use_imu, H_rows H_rows 1; end if use_wheel, H_rows H_rows 1; end Z zeros(H_rows, num_sigma); row_idx 1; if use_gps Z(row_idx, :) Xi_pred(1, :); Z(row_idx1, :) Xi_pred(2, :); row_idx row_idx 2; end if use_imu Z(row_idx, :) Xi_pred(5, :); row_idx row_idx 1; end if use_wheel Z(row_idx, :) Xi_pred(3, :); row_idx row_idx 1; end z_pred zeros(H_rows, 1); for i 1:num_sigma z_pred z_pred Wm(i) * Z(:, i); end % 量测协方差和互协方差 Pz R; Pxz zeros(n, H_rows); for i 1:num_sigma dz Z(:, i) - z_pred; dx Xi_pred(:, i) - x_pred; Pz Pz Wc(i) * (dz * dz); Pxz Pxz Wc(i) * (dx * dz); end % 卡尔曼增益和状态更新 K Pxz / Pz; x_est x_pred K * (z - z_pred); P_est P_pred - K * Pz * K; end可以看出UKF代码里完全没有求偏导数的环节整个流程就是生成Sigma点—传播—加权统计非常好维护。当你换了更复杂的非线性模型时只要改传播函数即可滤波器框架本身就一直不变。4.4 EKF和UKF的对比什么时候换用UKF收益最大从理论精度上说EKF只保留一阶项UKF可以隐含保留到二阶项。在强非线性场景下UKF的均值和协方差传播更准确。实际工程中我给出的选择建议如下表对比维度EKFUKF实现复杂度需要推导雅可比矩阵出错概率高免求导模型函数复用计算开销较低较高每步需传播2n1个点强非线性适配一阶近似转弯剧烈时可能发散二阶精度适应性强协方差数值稳定性一般需要保证P正定必要时做修复代码维护成本修改模型后需重推导数修改模型后直接复用框架从我实际测试的数据来看在车辆运动估计这个场景下如果车辆只是在中低速直线巡航EKF和UKF的估计误差差别在3%以内。但当车辆进行S型绕桩或急转弯时UKF的位置RMSE均方根误差比EKF低10%到20%而且EKF在某些场景下出现协方差收缩过快导致后续量测被无视的问题。如果你的应用场景涉及激烈驾驶、频繁变道或者漂移工况我更推荐直接用UKF。如果模型不复杂且转速稳定EKF够用计算开销也小一些。5. Simulink模型搭建与Matlab脚本的协作架构在Matlab/Simulink环境中做车辆状态估计有一个经典问题是在纯脚本里做仿真还是在Simulink里搭模型又或者是两者结合。我的建议是使用Simulink做传感器数据流和车辆动力学模型的可视化架构使用MATLAB Function块嵌入EKF/UKF滤波器的核心计算逻辑。这样既有模块化界面又方便调试滤波算法代码。5.1 Simulink模型整体架构一个完整的车辆状态估计Simulink模型至少包含以下几个模块车辆动力学模型或运动学模型用于生成车辆的真实状态轨迹作为ground truth传感器模型给真实状态叠加噪声生成GPS位置量测、IMU角速度量测、轮速量测EKF/UKF滤波器模块MATLAB Function块输入量测和上一个周期的状态估计输出当前时刻的状态估计可视化模块Scope显示估计值与真值对比或Record模块导出数据到工作空间前两个模块是数据来源第三个是核心算法第四个是验证手段。我建议把滤波器单独放到一个MATLAB Function块里并用Enable信号控制它是否启用。这样你可以很方便地对比有滤波和无滤波的效果——直接断开Enable信号即可。传感器模型的典型做法是z_gps true_state(1:2) sqrt(R_gps) * randn(2, 1); z_imu true_state(5) sqrt(R_imu) * randn(1, 1); z_wheel true_state(3) sqrt(R_wheel) * randn(1, 1);注意在Simulink的离散模型中一定要设置合适的采样时间。GPS采样频率一般是1~10HzIMU是50~100Hz轮速传感器是10~50Hz。多传感器不同采样频率下的融合问题本身就是一个大话题。如果你的滤波器是以最高频率运行的那么多余时间点上可以用上一次量测值或者干脆跳过量测更新。我的做法是给滤波器设置一个主采样时间如0.01s在每个仿真步长中检查是否有新的量测到达有才进行量测更新否则只做预测。这种事件驱动更新在Simulink中可以用触发子系统或简单的条件判断来实现。5.2 MATLAB Function块内部的结构设计MATLAB Function块内部建议不要直接写一大段非线性代码而是拆成几个子函数。Simulink支持在同一个MATLAB Function块中定义多个局部函数也可以调用外部函数文件。我的习惯是在Function块里定义入口函数在里面调用外部文件中实现好的ekf_predict_update或ukf_predict_update。需要注意的一个坑是MATLAB Function块在代码生成模式下比如你要生成C代码部署到嵌入式平台对外部函数的支持有限要求函数必须是纯MATLAB代码且数据类型可推断。如果只是做仿真验证那直接用外部函数没有任何问题。但如果你计划把模型转换为C代码建议把滤波器核心代码直接放到Function块内部而不是外部调用避免代码生成报错。另外在Function块中矩阵的大小在仿真开始前要预设好对不定长度的量测向量处理尤其要小心最好使用固定矩阵长度加逻辑开关来适配不同传感器组合这就是前面把use_gps、use_imu、use_wheel做成开关的原因之一。5.3 Carsim与Simulink联合仿真的接口设计搜索热词里频繁出现Carsim与Simulink联合仿真这套流程确实很实用。Carsim可以提供高精度的整车动力学模型比简化的CTRV模型真实得多。在Carsim与Simulink联合仿真中Carsim作为被控对象输入油门、制动、转向信号输出车辆运动状态真实状态。Simulink里放传感器模型给真实状态加噪声和滤波器对噪声信号做估计。最终对比三个信号Carsim输出的真实轨迹、传感器的带噪轨迹、滤波器的估计轨迹。这是我个人非常推荐的一种验证方式因为它把滤波器的性能放在高保真动力学模型下检验而不是在自建的简化运动学模型下自娱自乐。接口设置上Carsim通过Simulink的S-Function块与Simulink交换数据。你需要在Carsim的I/O配置里指定输出通道常见的有车辆全局X/Y坐标、横摆角、横摆角速度、纵向车速、质心侧偏角等。在Simulink中Carsim S-Function的输出端口顺序必须与Carsim界面中的输出通道顺序严格一致这个我踩过坑一次我在Carsim界面里调整了输出顺序但Simulink里没有同步更新结果滤波器的输入数据全部错位估计值直接崩掉。建议在S-Function输出后面接一个Demux或Selector模块把各信号显式分离开并且加上颜色和标签注释方便排查。5.4 仿真步长与实时性约束EKF和UKF在Simulink中跑仿真时步长的选择直接影响仿真速度和稳定性。我的经验是如果车辆模型和滤波器都在同一采样步长下运行仿真速度最快但会导致滤波器量测更新频率与动力学模型完全一致无法模拟多传感器异步问题。更真实的做法是设置多速率仿真车辆模型用1ms或2ms的固定步长GPS模块用100ms的采样步长IMU用10ms的采样步长滤波器用10ms的步长运行。这样可以看到多速率融合对滤波性能的影响。Simulink中多速率仿真的求解器建议选择变步长或固定步长中的离散求解器不要选择连续求解器去求解包含离散事件模型的问题否则容易导致step size very small的警告。6. 噪声协方差整定与滤波器发散排查实录滤波器调参是让人最头疼的部分。很多人的仿真跑不通不是算法原理没懂而是Q和R的取值不合理。这一章把我实际的调参方法和遇到的典型故障排查过程完整记录下来希望你能少走弯路。6.1 Q矩阵和R矩阵的物理含义与初始取值Q矩阵描述模型不确定性R矩阵描述传感器测量不确定性。一个常见误区是两个矩阵都是协方差矩阵所以数值越大代表噪声越大。这个理解没错但关键在于Q和R的相对比值决定滤波器的行为Q相对于R偏大滤波器非常相信量测数据状态估计会快速跟踪量测但会放大噪声轨迹不够平滑。Q相对于R偏小滤波器非常相信模型预测量测的修正作用很小可能出现量测更新不起效的情况即估计残差持续变大但滤波器不收敛。Q和R的比值靠经验调整没有一个绝对的正确值。初值设置上R矩阵可以直接通过传感器标称精度来设定。比如GPS水平位置标准差为2米那么R_gps diag([4, 4])注意是方差标准差的平方。IMU角速度的标准差如果是0.1 rad/sR_imu 0.01。轮速传感器标准差如果是0.3 m/sR_wheel 0.09。这些值有明确物理含义可以直接用。Q矩阵相对抽象。CTRV模型下需要设定σ_a和σ_ω。初始值建议室内低速车辆σ_a 0.3 m/s²σ_ω 0.3 rad/s²城市道路乘用车σ_a 0.8 m/s²σ_ω 0.5 rad/s²激烈驾驶/赛道工况σ_a 2.0 m/s²σ_ω 1.5 rad/s²如果你是第一次跑我建议把这些值作为基准然后观察仿真结果中的残差序列来调整。残差innovation是量测值减去量测预测值innovation z - h(x_pred)理论上如果滤波器工作正常innovation的均值应该接近零且其协方差应该与 S HPH R 一致。如果innovation明显有偏或协方差远大于S的对角线元素说明Q设小了模型没有跟上真实运动如果innovation的波动非常小且估计轨迹比真值还要平滑很多说明R设大了滤波器过于依赖模型预测。6.2 排查链路一协方差矩阵迅速收缩导致滤波器锁死现象滤波器运行一段时间后估计结果不再跟踪真实状态估计轨迹逐渐偏离但P矩阵的对角元素变得非常小看起来像滤波器认为自己已经收敛了。链路分析第一步检查innovation残差。如果innovation持续偏离零且数值较大说明滤波器没有正确融合量测。第二步检查卡尔曼增益K。如果K矩阵的数值变得接近零说明滤波器对量测的修正完全失效。第三步检查P_pred的计算。P_pred F * P_est * F Q如果Q设置过小P_pred会越来越小导致K越来越接近零量测修正信号完全被忽略。我遇到过的最典型原因有两个一是Q矩阵对角元素设置过小比R小几个数量级导致模型预测的协方差收缩到极小二是P矩阵初始值设置过小滤波器一开始就过度自信后面无法修正。解决方法是把Q适当调大并设置一个合理的P初始值一般取比R大一到两个量级即可比如P_init diag([5, 5, 1, 0.1, 0.1])这样初始的不确定性就足够大滤波器在开始时能更好地吸收量测信息。6.3 排查链路二角度环绕wrapping问题导致航向角跳变现象仿真过程中航向角估计在±π附近出现剧烈跳变滤波器发散。链路分析车辆的航向角本质上是周期性角度范围通常定义在[-π, π)或[0, 2π)。当真实航向角从π-0.01变化到-π0.01时数值上看起来像跳变了约2π。如果滤波器没有对角度做环绕处理计算innovation和差diff时会出现巨大误差。比如量测值是179度预测值是-179度误差不是-358度而是2度。如果滤波器直接把-358度当误差用状态修正会出错。这个问题的根源在于状态向量中的ψ是一个角度量任何涉及角度差的计算都要先做wrap到[-π, π)的处理。解决办法是写一个wrap_angle函数function wrapped wrap_angle(angle) wrapped atan2(sin(angle), cos(angle)); end在计算innovation时对角度量测对应的残差先做wrap在计算UKF的Sigma点加权协方差时对涉及角度的状态分量也要做环绕处理在计算EKF的预测差值时同样要注意角度分量的wrap。这个坑非常隐蔽因为不涉及角度的状态位置、速度一切正常只有航向角在特定角度处突然爆炸。如果你发现估计结果在车辆转圈时出现周期性发散90%的可能是角度环绕问题。6.4 排查链路三多传感器量测数据时间戳未对齐现象GPS、IMU和轮速的数据都正常但滤波器估计结果的误差比使用单一传感器还要大。链路分析多传感器融合的一个重要前提是各量测数据在时间上是对齐的。如果GPS数据滞后100ms而滤波器在用当前时刻的IMU数据和100ms之前的GPS数据同时更新相当于量测方程中存在不一致性滤波器会偏向滞后的GPS导致估计结果滞后且误差增大。Simulink中这个问题通常来自信号延迟模块设置不当或传感器的不同采样时间导致的固有延迟。Carsim与Simulink联合仿真时尤其容易出现因为Carsim的数据输出频率不稳定有时需要做缓冲对齐。如果条件允许最好的办法是在滤波器运行时同时记录每个量测的时间戳然后根据时间戳确定哪个数据可以用于当前时刻的量测更新。工程上简化的处理办法是以最高频率的IMU数据为主对低频GPS数据做最近邻匹配也就是在GPS数据到达时打一个标志位在下一拍滤波计算时使用最新的GPS数据。如果GPS和IMU的时间偏差超过一个GPS周期就需要考虑时间同步补偿算法了比如机械臂或无人机领域常用的视觉-IMU对齐方法。7. 从仿真到实测一次典型实验的完整流程与结果分析理论再扎实最终还是要走到实验这一步。我把自己做过的一组典型的车辆状态估计实验完整流程记录在这里。实验条件一辆搭载RTK-GPS、轮速传感器和IMU的测试车辆在一条包含直道和弯道的封闭测试道路上行驶。采集了真实轨迹作为ground truth然后对GPS量测加入2米标准差的高斯噪声对IMU加入0.1 rad/s的角速度噪声对轮速加入0.3 m/s的噪声。7.1 数据采集与时间同步处理采集过程中要注意几个细节轮速传感器通常输出的是脉冲信号解码后得到的是车辆纵向速度但不同车轮在转弯时速度不同。如果只使用单轮轮速在转弯时会产生偏差。更合理的做法是取后轮左右两个轮速的平均值或使用非驱动轮的轮速。另外IMU的安装位置和方向要标定准确特别是横摆角速度的方向如果安装反了估计结果会出现系统性偏差。时间同步方面我在采集时把每路数据都加上了一个时间戳。RTK-GPS的输出频率为10Hz轮速为50HzIMU为100Hz。在离线处理阶段我以IMU的时间为基准对GPS和轮速做线性插值对齐。这里需要注意的是插值会引入一定误差但相对于传感器本身噪声来说可以忽略。如果你在线实时处理建议用缓冲区方式等待GPS新数据到达后再进行一次量测更新不要强制把低频传感器数据插值到高频那会在实时系统中引入额外延迟。7.2 滤波性能指标与结果对比评估滤波器性能我一直使用两个基本指标RMSE均方根误差和最大绝对误差。分别对位置、速度、航向角三个核心状态计算。指标EKFUKF位置RMSE (m)1.871.54位置最大误差 (m)4.323.56速度RMSE (m/s)0.350.29航向角RMSE (rad)0.0740.052从这组结果看UKF在相同噪声条件下的位置RMSE比EKF低约17%航向角RMSE低近30%。这符合理论预期转弯工况中航向角的非线性最强UKF的二阶近似优势体现得最明显。真实工况与纯仿真在性能上还是有明显差异的。仿真中传感器噪声是理想高斯白噪声真实环境中的GPS多路径效应、轮速打滑、IMU温漂都会导致实际RMSE比仿真结果更大且可能出现连续数秒的误差高峰。我第一次把算法从仿真搬到实车数据上时位置误差直接比仿真翻了一倍后来仔细检查发现是轮速打滑导致的。直道上车辆急加速时驱动轮空转轮速传感器给出的速度远大于真实车速导致速度估计短暂偏差较大进而影响位置估计。解决思路是增大过程噪声中的σ_a值让滤波器不盲目相信模型预测同时可以把轮速量测的R设成动态值——检测到急加速时增大R的值降低轮速传感器在打滑工况下的可信度。这个做法本质上是一种简单的自适应噪声调整思想在很多场景下非常实用。8. 几个容易忽略的工程细节与参数设置建议最后这部分补充一些比较零碎但直接影响代码能否跑通或结果是否合理的经验点都是我实际动手过程中踩过的。第一P矩阵的初始化很关键不能随便设零矩阵。如果P_init全部为0滤波器从一开始就没有不确定性之后无论量测和过程怎么更新状态都不会改变多少。合理的初始P要反映你对初始状态的信任程度。如果你用第一个GPS位置和IMU数据来初始化状态位置方差直接用GPS精度即可速度可以用轮速精度航向角用IMU初始值精度。这样初始化之后滤波器一开始就处于较合理的置信区间。第二EKF的雅可比矩阵和UKF的Sigma点传播都要保证状态更新后的角度分量落在合理范围内否则会出现角度累积漂移。我在代码里习惯在每个滤波周期结束后对ψ做一次wrap处理。虽然滤波器内部计算用的是矩阵但状态量本身的角度属性需要显式处理不要等着它自己绕回来极容易绕成很大的角度影响后续的三角函数计算。第三Matlab代码里凡是涉及除法的地方都要检查分母为零的边界条件尤其是CTRV模型中 v/ψ_dot 这一项。如果你把ψ_dot的初始值设成0这个除法直接导致无穷大仿真必然发散。我开始用的是直接赋值后来改成阈值分支判断才把问题解决。这个阈值选多大也有讲究如果选得太大比如1e-2那么车辆在慢转弯时会被错误地当作直线运动处理位置估计会走直线与真实圆弧轨迹不符我实际测试下来 1e-6 到 1e-4 都是合理的范围建议取1e-6。第四Simulink仿真时间和步长的选择直接影响结果的数值稳定性。固定步长求解器建议选择ode4或ode5步长设为采样时间的最小公因数这样各模块的量测和状态更新才能严格对齐。如果你要用变步长务必设置最大步长限制否则Simulink可能会在状态变化平缓时使用很大的步长导致滤波器的量测更新不连续。另一个常被忽视的点是MATLAB Function块中如果使用randn生成量测噪声每次运行仿真会得到不同的噪声序列导致结果不可复现。建议设置随机数种子或者用一个单独的Constant模块噪声vector存储的方式把量测噪声固定下来这样调试时才能对比不同滤波器参数的实际差异。第五关于实时性优化。EKF的计算量集中在雅可比矩阵求值和矩阵乘法UKF的计算量集中在Sigma点传播和协方差加权。两者在Matlab环境下跑离线仿真没有任何性能压力但如果要移植到嵌入式平台建议把矩阵运算尽量改写成固定大小的运算避免动态内存分配。CTRV模型5维状态量对应5x5矩阵在C语言里用静态数组实现完全没有问题。UKF需要传播11个Sigma点2*51每个点都要调用一次过程模型和量测模型在MCU上可能会比较吃力这时可以尝试降维处理的传感器融合策略比如把位置和姿态分离成两个子滤波器而不是一个5维大滤波器。每次回头做这套流程我的体会是EKF和UKF的公式都不算复杂真正的难点永远在模型选择、噪声参数标定和工程细节处理上。如果你现在也在做行驶车辆状态估计希望这篇内容能帮你把前面的路看得更清楚。 EKF与UKF在车辆状态估计中的应用：从CTRV模型到Matlab/Simulink实现
车还没拐弯估计的位置已经飘到隔壁车道去了——这是我第一次用线性卡尔曼滤波做车辆状态估计时遇到的真实情况。后来把模型换成CTRV恒定转率和速度运动学模型线性卡尔曼直接失效因为状态转移方程里的正弦余弦项没办法用线性矩阵表达。那之后我花了很长时间把扩展卡尔曼滤波EKF和无迹卡尔曼滤波UKF都完整实现了一遍在Matlab和Simulink环境里搭了一套可复用的车辆状态估计框架。这篇内容算是把整个过程的思路、公式推导、代码细节和踩坑记录都整理出来给正在做类似工作的朋友一个参考。这个方向在工程上的典型应用场景包括自动驾驶中的车辆定位与轨迹预测、车辆动力学参数实时辨识、车载传感器GPS、IMU、轮速传感器的信号融合、车队控制中的前车状态估计等。无论你在做哪个方向只要涉及根据带噪声的传感器数据实时推算车辆的真实运动状态EKF/UKF就是绕不开的两种经典方案。本文会用Matlab代码和Simulink模型两种形式展示完整实现并重点说明为什么在某些场景下UKF比EKF更稳以及噪声协方差矩阵到底该怎么调。1. 为什么线性卡尔曼滤波处理不了车辆运动估计先看一个非常典型的场景车辆在平面上运动状态量取为位置x、位置y、速度大小v、航向角ψ和航向角变化率ψ_dot这就是CTRV模型。它的连续时间状态方程是dx/dt v * cos(ψ) dy/dt v * sin(ψ) dv/dt 0 假设加速度为零实际通过过程噪声吸收 dψ/dt ψ_dot dψ_dot/dt 0如果系统是线性的状态转移方程可以写成 x_k A * x_{k-1} B * u_k w_k然后直接用标准卡尔曼滤波的预测和更新两步就能搞定。但你看这个模型里的 cos(ψ) 和 sin(ψ)它们是状态 ψ 的非线性函数。写成离散形式后状态转移本身就没有办法用一个常矩阵 A 来表示。整个系统是非线性的标准卡尔曼滤波在这个模型下根本不适用。有的朋友可能会想那我用恒速度CV模型把状态量改成 x方向速度vx和y方向速度vy不就没有三角函数了吗。确实可以但这样等于放弃了航向角这个关键状态量。在车辆运动估计中航向角恰恰是最重要的状态之一无论是做轨迹预测还是做传感器融合你都需要知道车头的朝向角。而且CV模型的线性化假设太强稍微遇到一个弯道位置预测就会出现明显偏移。非线性的另一个来源是量测方程。你用GPS观测车辆位置观测值一般是经纬度转换后的平面坐标这个观测方程还算简单是线性的。但如果你用雷达或毫米波雷达观测量测往往是目标相对于雷达的距离和方位角。距离方程是 sqrt((px - rx)^2 (py - ry)^2)方位角是 atan2(py - ry, px - rx)这两个都是状态量的非线性函数。就算状态转移可以勉强近似成线性量测方程也会引入非线性。非线性系统的最优状态估计在数学上没有一个闭式解——也就是说你没有办法像线性卡尔曼那样用一组简洁的矩阵递推公式精确算出后验分布。EKF和UKF本质上都是一种次优近似它们的核心思路都是把非线性问题变成可以算的问题。EKF的做法是对非线性函数做一阶泰勒展开用雅可比矩阵来近似UKF的做法是用一组采样点Sigma点直接进行非线性变换然后从变换后的点中恢复均值和协方差。两种思路各有优劣后面章节会展开讲。2. 车辆模型的取舍状态向量、过程方程与量测方程的设计逻辑做状态估计的第一步永远不是写代码而是先确定状态向量里面有什么过程模型怎么走量测模型是什么。这三者直接决定滤波器的结构和最终效果。2.1 状态向量怎么选从CTRV模型说起我这次采用的是CTRV模型状态向量是x [px; py; v; ψ; ψ_dot]其中 px、py 是车辆在全局坐标系下的平面位置v 是纵向车速ψ 是航向角ψ_dot 是航向角变化率也就是横摆角速度。为什么选这五个量因为它们在车辆运动学中构成了一个自洽的闭环位置的变化率由速度和航向角决定航向角的变化率由 ψ_dot 决定。系统输入中没有加速度项而是通过过程噪声来吸收车辆的加速和减速这是CTRV的标准做法。相比直接加入加速度项的constant accelerationCA模型CTRV在转弯场景下有更好的适应性因为ψ_dot是显式的状态量模型天然支持圆弧运动轨迹。过程方程的离散形式为假设采样间隔为 Δtpx_{k1} px_k (v_k / ψ_dot_k) * (sin(ψ_k ψ_dot_k * Δt) - sin(ψ_k)) 当 |ψ_dot_k| eps px_{k1} px_k v_k * cos(ψ_k) * Δt 当 |ψ_dot_k| ≈ 0py_{k1} py_k (v_k / ψ_dot_k) * (-cos(ψ_k ψ_dot_k * Δt) cos(ψ_k)) 当 |ψ_dot_k| eps py_{k1} py_k v_k * sin(ψ_k) * Δt 当 |ψ_dot_k| ≈ 0v_{k1} v_k ψ_{k1} ψ_k ψ_dot_k * Δt ψ_dot_{k1} ψ_dot_k这里有个重要的工程细节当 ψ_dot 趋近于零时包含除法的公式会出现数值问题所以必须做一次条件判断退化成直线运动模型。这个细节在实际代码里非常关键很多新手第一次跑CTRV模型发散就是没处理这个边界条件。我建议在代码里先做一个阈值判断比如 |ψ_dot| 1e-6 就走直线模型分支。2.2 过程噪声的建模Q矩阵不是拍脑袋定的过程噪声协方差矩阵 Q 反映的是模型本身的误差。CTRV模型的误差来源主要有两个纵向加速度和横摆角速度的角加速度。也就是说模型认为 v 和 ψ_dot 是常量但实际上车辆会加速、会转向这个模型误差需要通过过程噪声来覆盖。通常做法是设定两个噪声参数直线加速度噪声标准差 σ_a 和角加速度噪声标准差 σ_ω。把连续时间的加速度噪声映射到状态空间得到过程噪声协方差矩阵。由于过程方程是v_k和ψ_dot_k的非线性函数严格的离散化处理需要计算噪声对各个状态的线性化传递矩阵。工程上常用的简化方式是GPTGeneralized Pseudo-Transformation近似——通过直接对状态转移方程求偏导推得Q矩阵的具体形式。对CTRV模型常见的Q矩阵形式是G [0.5 * Δt^2 * cos(ψ) , 0 0.5 * Δt^2 * sin(ψ) , 0 Δt , 0 0 , 0.5 * Δt^2 0 , Δt]Q G * diag([σ_a^2, σ_ω^2]) * G这个矩阵里每一项都有物理意义位置项的噪声与时间平方成正比因为加速度误差对位置的影响是二次积分速度项与时间一次方成正比横摆角速度同理。说实话这个Q矩阵是最难调的参数之一因为σ_a和σ_ω本身没有一个精确的物理测定方法更多是靠经验结合传感器特性来整定。后面的调参章节会专门讲怎么调。2.3 量测方程的设计GPS与IMU的量测特性差异量测模型取决于你手上的传感器。我做的系统里有两类量测可用GPS位置量测直接观测 px 和 py。量测方程是线性的z [px; py] R_gpsR_gps 是GPS的量测噪声协方差它的量级取决于GPS的精度。普通民用GPS精度在2~5米左右差分GPSRTK可以到厘米级。如果你需要在自动驾驶级别的精度RTK基本是标配但这不在本文讨论范围内日常实验用普通GPS数据看趋势完全够用。IMU量测IMU可以提供加速度和角速度。在本文的CTRV模型中我们主要用IMU提供的横摆角速度 ψ_dot_imu 和纵向加速度数据。如果直接把 ψ_dot_imu 当作量测量测方程就是z_imu ψ_dot R_imu也就是说IMU的观测值直接与状态向量中的ψ_dot对应。有些实现方案会把IMU的加速度作为控制输入而不是量测两种方案都可行。我的做法是把它作为量测来处理这样可以利用量测更新来修正对ψ_dot的估计而不是仅仅把它当作输入然后靠过程噪声去匹配。还有一个在车辆导航中常见的量测是轮速传感器提供的车速 v_wheel。同样地我们可以把它直接当作v状态量的量测。在设计量测方程时要特别注意各传感器的坐标系是否一致。GPS给的是全局坐标系ENU东北天下的坐标IMU给的是车体坐标系下的数据轮速传感器给的是车体纵向速度。如果你不加处理直接把IMU的加速度作为状态量对应的量测大概率会出问题。工程做法是对IMU数据做坐标变换把车体坐标系的角速度映射到导航坐标系对轮速数据则直接使用因为它本身就是纵向速度。这个坐标系问题在后面的Simulink实现部分还会再次出现这是一个非常容易被忽略的坑。3. EKF实现拆解从雅可比矩阵到状态递推的完整推导EKF扩展卡尔曼滤波是处理非线性系统最经典的方法思路简单粗暴把非线性函数在状态估计点附近做一阶泰勒展开然后套用标准卡尔曼滤波框架。因为只保留了一阶项所以EKF的精度取决于非线性函数的非线性程度。车辆运动模型中的三角函数虽然是非线性的但在小采样间隔下一阶展开的误差通常可控这也是EKF至今仍然大量使用的原因。3.1 EKF的预测与更新公式EKF的两步结构与标准卡尔曼滤波器完全一致只是把原有的矩阵乘法换成了非线性函数的求值加雅可比矩阵的传递预测步x_pred f(x_est) P_pred F * P_est * F Q更新步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这里的 F 是状态转移函数 f 对状态向量 x 的雅可比矩阵H 是量测函数 h 对状态向量 x 的雅可比矩阵。一切都围绕这两个矩阵展开。3.2 状态转移雅可比矩阵 F 的推导注意一个关键点雅可比矩阵是在当前预测点求值的。也就是说虽然非线性函数形式不变但矩阵的数值每个滤波周期都在变化必须每步重新计算。你在写代码时不能把F当成固定常数矩阵。对CTRV模型雅可比矩阵F5x5各元素是对状态向量各分量的偏导数。设:a (v / ψ_dot) * (sin(ψ ψ_dot * Δt) - sin(ψ)) b (v / ψ_dot) * (-cos(ψ ψ_dot * Δt) cos(ψ))即 px_{k1} px apy_{k1} py b。可以得到∂px_{k1}/∂px 1 ∂px_{k1}/∂py 0 ∂px_{k1}/∂v (sin(ψ ψ_dotΔt) - sin(ψ)) / ψ_dot ∂px_{k1}/∂ψ (v/ψ_dot) * (cos(ψ ψ_dotΔt) - cos(ψ)) ∂px_{k1}/∂ψ_dot -(v/ψ_dot^2) * (sin(ψ ψ_dotΔt) - sin(ψ)) (v/ψ_dot) * cos(ψ ψ_dotΔt) * Δt∂py_{k1}/∂px 0 ∂py_{k1}/∂py 1 ∂py_{k1}/∂v (-cos(ψ ψ_dotΔt) cos(ψ)) / ψ_dot ∂py_{k1}/∂ψ (v/ψ_dot) * (sin(ψ ψ_dotΔt) - sin(ψ)) ∂py_{k1}/∂ψ_dot (v/ψ_dot^2) * (cos(ψ ψ_dotΔt) - cos(ψ)) (v/ψ_dot) * sin(ψ ψ_dotΔt) * Δt∂v_{k1}/∂v 1其余为0 ∂ψ_{k1}/∂ψ 1∂ψ_{k1}/∂ψ_dot Δt ∂ψ_dot_{k1}/∂ψ_dot 1这个形式比较长我再次提醒一个关键点以上是ψ_dot ≠ 0时的形式。当|ψ_dot|极小时有些分母会变成无穷大必须在代码里做分支处理退化成直线运动模型后重新求偏导。直线模型的雅可比就是标准CV模型的形式简单很多。如果你偷懒不处理这个细节仿真中车辆一旦直线行驶数值就会异常。实际编码时我更建议用MATLAB的Symbolic Math Toolbox一次性导出符号雅可比矩阵然后再生成数值函数。当年我手推这个矩阵推错了好几次用了符号工具箱之后效率高了很多。可以这么写% 使用符号计算导出雅可比矩阵 syms px py v psi psi_dot dt % 定义状态转移函数 f [px (v/psi_dot)*(sin(psi psi_dot*dt) - sin(psi)); py (v/psi_dot)*(-cos(psi psi_dot*dt) cos(psi)); v; psi psi_dot*dt; psi_dot]; % 计算雅可比矩阵 F_sym jacobian(f, [px, py, v, psi, psi_dot]); % 导出为MATLAB函数 matlabFunction(F_sym, File, compute_F_matrix, Vars, {px, py, v, psi, psi_dot, dt});这样生成的 compute_F_matrix 函数可以直接被EKF的代码调用不会符号推导错误。3.3 量测雅可比矩阵 H 与滤波器代码实现对于GPS位置量测量测方程为h1(x) px h2(x) py所以H矩阵为H [1 0 0 0 0 0 1 0 0 0]如果加入IMU横摆角速度量测就把h3(x) ψ_dot 加进去对应H的第3行为 [0 0 0 0 1]。如果加入轮速量测h4(x) v对应H的第4行为 [0 0 1 0 0]。这个H矩阵是线性的不需要求偏导但要注意量测维度和噪声协方差矩阵R的维度要保持一致。以下是完整的EKF算法代码我已经在Matlab中验证过function [x_est, P_est] ekf_predict_update(x_est, P_est, z, dt, Q, R, use_gps, use_imu, use_wheel) % EKF预测步 psi_dot x_est(5); v x_est(4); psi x_est(3); % 根据psi_dot选择模型 if abs(psi_dot) 1e-6 % 直线运动模型 F [1 0 dt*cos(psi) -v*dt*sin(psi) 0; 0 1 dt*sin(psi) v*dt*cos(psi) 0; 0 0 1 0 0; 0 0 0 1 dt; 0 0 0 0 1]; x_pred x_est [v*cos(psi)*dt; v*sin(psi)*dt; 0; 0; 0]; else % 圆弧运动模型 F compute_F_matrix(x_est(1), x_est(2), v, psi, psi_dot, dt); dp v/psi_dot; x_pred [x_est(1) dp*(sin(psi psi_dot*dt) - sin(psi)); x_est(2) dp*(-cos(psi psi_dot*dt) cos(psi)); v; psi psi_dot*dt; psi_dot]; end P_pred F * P_est * F Q; % 构造量测向量和量测矩阵 H []; z_meas []; R_meas []; if use_gps H [H; 1 0 0 0 0; 0 1 0 0 0]; z_meas [z_meas; z(1); z(2)]; R_meas blkdiag(R_meas, R(1:2, 1:2)); end if use_imu H [H; 0 0 0 0 1]; z_meas [z_meas; z(3)]; R_meas blkdiag(R_meas, R(3, 3)); end if use_wheel H [H; 0 0 1 0 0]; z_meas [z_meas; z(4)]; R_meas blkdiag(R_meas, R(4, 4)); end % 更新步 S H * P_pred * H R_meas; K P_pred * H / S; x_est x_pred K * (z_meas - H * x_pred); P_est (eye(5) - K * H) * P_pred; end这个函数每次需要传入量测向量z并根据开关值决定使用哪些传感器。这种设计的好处是后续做传感器故障注入或效果对比时非常方便你不需要改写核心滤波器只要切换开关即可。3.4 EKF的局限性一阶展开带来的精度瓶颈EKF最大的问题在于一阶泰勒展开的近似能力有限。如果非线性的强度大比如ψ_dot很大、航向角快速变化时一阶近似会让均值和协方差的传播出现偏差。另一个问题是雅可比矩阵的计算容易出错尤其是比较复杂的模型手推偏导数非常容易漏项或算错符号。EKF还有一个隐含假设噪声必须服从高斯分布且经过非线性变换后仍近似高斯。如果系统中存在强非高斯噪声EKF的表现会明显下降。对车辆运动估计来说GPS的噪声在有遮挡物时会出现重尾特性此时EKF的性能会受到明显影响这也是后面要引入UKF的现实动机之一。4. UKF实现拆解Sigma点怎么选、权重怎么分、更新怎么做UKF和无迹变换Unscented Transform的设计哲学和EKF完全不同。它不计算雅可比矩阵而是找一组精心选择的采样点Sigma点让这些点经过非线性函数传播后用加权统计的方式恢复出均值和协方差。这样做的好处是不需要解析求导而且对非线性函数的近似精度能到二阶甚至更高因为Sigma点传播保留了更多非线性信息。4.1 无迹变换的原理无迹变换的核心思想是对n维高斯分布选取2n1个Sigma点这些点的均值和协方差与原始分布完全一致。将这些点分别通过非线性函数传播后得到的点集能够反映非线性变换后分布的均值和协方差特征。你可以把它理解为用一群点的分布去近似一个分布的统计特性而不是去近似非线性函数本身。Sigma点的生成公式为χ(0) x_mean χ(i) x_mean sqrt((n λ) * P) 的第i列i 1..n χ(in) x_mean - sqrt((n λ) * P) 的第i列i 1..n其中 λ α^2 * (n κ) - nα控制Sigma点相对于均值的扩散程度一般取一个较小的正值如0.001到1之间κ是次级缩放参数在高斯分布下通常取0或3-n。权重为Wm(0) λ / (n λ) Wc(0) λ / (n λ) (1 - α^2 β) Wm(i) Wc(i) 1 / (2 * (n λ))i 1..2nβ一般取2对应高斯分布下的最优值。每次计算sqrt((n λ) * P)时必须用Cholesky分解或矩阵平方根且要求P是正定矩阵。这个正定要求在实际仿真中很关键因为协方差矩阵经过多次递推后可能因为数值误差失去正定性。如果你在跑UKF时出现Matrix must be positive definite的报错大概率就是协方差矩阵被迭代坏了需要在每个周期做一次对称正定修复或使用平方根UKF。4.2 Sigma点生成与权重计算代码以下是在Matlab中实现Sigma点生成的完整代码function [Xi, Wm, Wc] generate_sigma_points(x, P, alpha, beta, kappa) n length(x); lambda alpha^2 * (n kappa) - n; % 计算矩阵平方根 sqrt_matrix chol((n lambda) * P, lower); % 生成Sigma点 Xi zeros(n, 2*n1); Xi(:, 1) x; for i 1:n Xi(:, i1) x sqrt_matrix(:, i); Xi(:, i1n) x - sqrt_matrix(:, i); end % 权重 Wm zeros(2*n1, 1); Wc zeros(2*n1, 1); Wm(1) lambda / (n lambda); Wc(1) lambda / (n lambda) (1 - alpha^2 beta); for i 2:(2*n1) Wm(i) 1 / (2 * (n lambda)); Wc(i) 1 / (2 * (n lambda)); end end这里用 chol 函数做Cholesky分解代替了sqrtm因为chol的计算效率更高且对正定矩阵更稳定。如果遇到chol分解失败的情况建议先对P做对称化处理 P (P P)/2再尝试分解。如果仍然失败可能是过程噪声Q设计过小导致协方差收敛到奇异需要检查Q的取值。4.3 Sigma点传播与量测更新UKF的预测和更新都围绕Sigma点展开。预测步将Sigma点逐一通过状态转移函数传播然后加权计算预测均值和协方差function [x_pred, P_pred] ukf_predict(Xi, Wm, Wc, dt, Q) n size(Xi, 1); num_sigma size(Xi, 2); % 传播Sigma点 Xi_pred zeros(n, num_sigma); for i 1:num_sigma Xi_pred(:, i) ctrv_process_model(Xi(:, i), dt); end % 加权计算预测均值 x_pred zeros(n, 1); for i 1:num_sigma x_pred x_pred Wm(i) * Xi_pred(:, i); end % 加权计算预测协方差 P_pred Q; for i 1:num_sigma diff Xi_pred(:, i) - x_pred; P_pred P_pred Wc(i) * (diff * diff); end endctrv_process_model 函数就是前文提到的CTRV离散状态转移函数这里不再重复实现但注意它同样要处理ψ_dot趋近于零的分支情况。UKF和EKF共用同一个过程模型函数这也是UKF的一个工程优势——你不需要额外推导雅可比矩阵。量测更新与预测是类似的过程先用量测函数传播Sigma点然后计算量测预测均值、新息协方差和互协方差最后计算卡尔曼增益并更新状态和协方差function [x_est, P_est] ukf_update(x_pred, P_pred, Xi_pred, Wm, Wc, z, R, use_gps, use_imu, use_wheel) n length(x_pred); num_sigma size(Xi_pred, 2); % 构建量测函数输出 % 假设量测包含 [px; py; psi_dot; v] Z zeros(4, num_sigma); H_rows 0; if use_gps, H_rows H_rows 2; end if use_imu, H_rows H_rows 1; end if use_wheel, H_rows H_rows 1; end Z zeros(H_rows, num_sigma); row_idx 1; if use_gps Z(row_idx, :) Xi_pred(1, :); Z(row_idx1, :) Xi_pred(2, :); row_idx row_idx 2; end if use_imu Z(row_idx, :) Xi_pred(5, :); row_idx row_idx 1; end if use_wheel Z(row_idx, :) Xi_pred(3, :); row_idx row_idx 1; end z_pred zeros(H_rows, 1); for i 1:num_sigma z_pred z_pred Wm(i) * Z(:, i); end % 量测协方差和互协方差 Pz R; Pxz zeros(n, H_rows); for i 1:num_sigma dz Z(:, i) - z_pred; dx Xi_pred(:, i) - x_pred; Pz Pz Wc(i) * (dz * dz); Pxz Pxz Wc(i) * (dx * dz); end % 卡尔曼增益和状态更新 K Pxz / Pz; x_est x_pred K * (z - z_pred); P_est P_pred - K * Pz * K; end可以看出UKF代码里完全没有求偏导数的环节整个流程就是生成Sigma点—传播—加权统计非常好维护。当你换了更复杂的非线性模型时只要改传播函数即可滤波器框架本身就一直不变。4.4 EKF和UKF的对比什么时候换用UKF收益最大从理论精度上说EKF只保留一阶项UKF可以隐含保留到二阶项。在强非线性场景下UKF的均值和协方差传播更准确。实际工程中我给出的选择建议如下表对比维度EKFUKF实现复杂度需要推导雅可比矩阵出错概率高免求导模型函数复用计算开销较低较高每步需传播2n1个点强非线性适配一阶近似转弯剧烈时可能发散二阶精度适应性强协方差数值稳定性一般需要保证P正定必要时做修复代码维护成本修改模型后需重推导数修改模型后直接复用框架从我实际测试的数据来看在车辆运动估计这个场景下如果车辆只是在中低速直线巡航EKF和UKF的估计误差差别在3%以内。但当车辆进行S型绕桩或急转弯时UKF的位置RMSE均方根误差比EKF低10%到20%而且EKF在某些场景下出现协方差收缩过快导致后续量测被无视的问题。如果你的应用场景涉及激烈驾驶、频繁变道或者漂移工况我更推荐直接用UKF。如果模型不复杂且转速稳定EKF够用计算开销也小一些。5. Simulink模型搭建与Matlab脚本的协作架构在Matlab/Simulink环境中做车辆状态估计有一个经典问题是在纯脚本里做仿真还是在Simulink里搭模型又或者是两者结合。我的建议是使用Simulink做传感器数据流和车辆动力学模型的可视化架构使用MATLAB Function块嵌入EKF/UKF滤波器的核心计算逻辑。这样既有模块化界面又方便调试滤波算法代码。5.1 Simulink模型整体架构一个完整的车辆状态估计Simulink模型至少包含以下几个模块车辆动力学模型或运动学模型用于生成车辆的真实状态轨迹作为ground truth传感器模型给真实状态叠加噪声生成GPS位置量测、IMU角速度量测、轮速量测EKF/UKF滤波器模块MATLAB Function块输入量测和上一个周期的状态估计输出当前时刻的状态估计可视化模块Scope显示估计值与真值对比或Record模块导出数据到工作空间前两个模块是数据来源第三个是核心算法第四个是验证手段。我建议把滤波器单独放到一个MATLAB Function块里并用Enable信号控制它是否启用。这样你可以很方便地对比有滤波和无滤波的效果——直接断开Enable信号即可。传感器模型的典型做法是z_gps true_state(1:2) sqrt(R_gps) * randn(2, 1); z_imu true_state(5) sqrt(R_imu) * randn(1, 1); z_wheel true_state(3) sqrt(R_wheel) * randn(1, 1);注意在Simulink的离散模型中一定要设置合适的采样时间。GPS采样频率一般是1~10HzIMU是50~100Hz轮速传感器是10~50Hz。多传感器不同采样频率下的融合问题本身就是一个大话题。如果你的滤波器是以最高频率运行的那么多余时间点上可以用上一次量测值或者干脆跳过量测更新。我的做法是给滤波器设置一个主采样时间如0.01s在每个仿真步长中检查是否有新的量测到达有才进行量测更新否则只做预测。这种事件驱动更新在Simulink中可以用触发子系统或简单的条件判断来实现。5.2 MATLAB Function块内部的结构设计MATLAB Function块内部建议不要直接写一大段非线性代码而是拆成几个子函数。Simulink支持在同一个MATLAB Function块中定义多个局部函数也可以调用外部函数文件。我的习惯是在Function块里定义入口函数在里面调用外部文件中实现好的ekf_predict_update或ukf_predict_update。需要注意的一个坑是MATLAB Function块在代码生成模式下比如你要生成C代码部署到嵌入式平台对外部函数的支持有限要求函数必须是纯MATLAB代码且数据类型可推断。如果只是做仿真验证那直接用外部函数没有任何问题。但如果你计划把模型转换为C代码建议把滤波器核心代码直接放到Function块内部而不是外部调用避免代码生成报错。另外在Function块中矩阵的大小在仿真开始前要预设好对不定长度的量测向量处理尤其要小心最好使用固定矩阵长度加逻辑开关来适配不同传感器组合这就是前面把use_gps、use_imu、use_wheel做成开关的原因之一。5.3 Carsim与Simulink联合仿真的接口设计搜索热词里频繁出现Carsim与Simulink联合仿真这套流程确实很实用。Carsim可以提供高精度的整车动力学模型比简化的CTRV模型真实得多。在Carsim与Simulink联合仿真中Carsim作为被控对象输入油门、制动、转向信号输出车辆运动状态真实状态。Simulink里放传感器模型给真实状态加噪声和滤波器对噪声信号做估计。最终对比三个信号Carsim输出的真实轨迹、传感器的带噪轨迹、滤波器的估计轨迹。这是我个人非常推荐的一种验证方式因为它把滤波器的性能放在高保真动力学模型下检验而不是在自建的简化运动学模型下自娱自乐。接口设置上Carsim通过Simulink的S-Function块与Simulink交换数据。你需要在Carsim的I/O配置里指定输出通道常见的有车辆全局X/Y坐标、横摆角、横摆角速度、纵向车速、质心侧偏角等。在Simulink中Carsim S-Function的输出端口顺序必须与Carsim界面中的输出通道顺序严格一致这个我踩过坑一次我在Carsim界面里调整了输出顺序但Simulink里没有同步更新结果滤波器的输入数据全部错位估计值直接崩掉。建议在S-Function输出后面接一个Demux或Selector模块把各信号显式分离开并且加上颜色和标签注释方便排查。5.4 仿真步长与实时性约束EKF和UKF在Simulink中跑仿真时步长的选择直接影响仿真速度和稳定性。我的经验是如果车辆模型和滤波器都在同一采样步长下运行仿真速度最快但会导致滤波器量测更新频率与动力学模型完全一致无法模拟多传感器异步问题。更真实的做法是设置多速率仿真车辆模型用1ms或2ms的固定步长GPS模块用100ms的采样步长IMU用10ms的采样步长滤波器用10ms的步长运行。这样可以看到多速率融合对滤波性能的影响。Simulink中多速率仿真的求解器建议选择变步长或固定步长中的离散求解器不要选择连续求解器去求解包含离散事件模型的问题否则容易导致step size very small的警告。6. 噪声协方差整定与滤波器发散排查实录滤波器调参是让人最头疼的部分。很多人的仿真跑不通不是算法原理没懂而是Q和R的取值不合理。这一章把我实际的调参方法和遇到的典型故障排查过程完整记录下来希望你能少走弯路。6.1 Q矩阵和R矩阵的物理含义与初始取值Q矩阵描述模型不确定性R矩阵描述传感器测量不确定性。一个常见误区是两个矩阵都是协方差矩阵所以数值越大代表噪声越大。这个理解没错但关键在于Q和R的相对比值决定滤波器的行为Q相对于R偏大滤波器非常相信量测数据状态估计会快速跟踪量测但会放大噪声轨迹不够平滑。Q相对于R偏小滤波器非常相信模型预测量测的修正作用很小可能出现量测更新不起效的情况即估计残差持续变大但滤波器不收敛。Q和R的比值靠经验调整没有一个绝对的正确值。初值设置上R矩阵可以直接通过传感器标称精度来设定。比如GPS水平位置标准差为2米那么R_gps diag([4, 4])注意是方差标准差的平方。IMU角速度的标准差如果是0.1 rad/sR_imu 0.01。轮速传感器标准差如果是0.3 m/sR_wheel 0.09。这些值有明确物理含义可以直接用。Q矩阵相对抽象。CTRV模型下需要设定σ_a和σ_ω。初始值建议室内低速车辆σ_a 0.3 m/s²σ_ω 0.3 rad/s²城市道路乘用车σ_a 0.8 m/s²σ_ω 0.5 rad/s²激烈驾驶/赛道工况σ_a 2.0 m/s²σ_ω 1.5 rad/s²如果你是第一次跑我建议把这些值作为基准然后观察仿真结果中的残差序列来调整。残差innovation是量测值减去量测预测值innovation z - h(x_pred)理论上如果滤波器工作正常innovation的均值应该接近零且其协方差应该与 S HPH R 一致。如果innovation明显有偏或协方差远大于S的对角线元素说明Q设小了模型没有跟上真实运动如果innovation的波动非常小且估计轨迹比真值还要平滑很多说明R设大了滤波器过于依赖模型预测。6.2 排查链路一协方差矩阵迅速收缩导致滤波器锁死现象滤波器运行一段时间后估计结果不再跟踪真实状态估计轨迹逐渐偏离但P矩阵的对角元素变得非常小看起来像滤波器认为自己已经收敛了。链路分析第一步检查innovation残差。如果innovation持续偏离零且数值较大说明滤波器没有正确融合量测。第二步检查卡尔曼增益K。如果K矩阵的数值变得接近零说明滤波器对量测的修正完全失效。第三步检查P_pred的计算。P_pred F * P_est * F Q如果Q设置过小P_pred会越来越小导致K越来越接近零量测修正信号完全被忽略。我遇到过的最典型原因有两个一是Q矩阵对角元素设置过小比R小几个数量级导致模型预测的协方差收缩到极小二是P矩阵初始值设置过小滤波器一开始就过度自信后面无法修正。解决方法是把Q适当调大并设置一个合理的P初始值一般取比R大一到两个量级即可比如P_init diag([5, 5, 1, 0.1, 0.1])这样初始的不确定性就足够大滤波器在开始时能更好地吸收量测信息。6.3 排查链路二角度环绕wrapping问题导致航向角跳变现象仿真过程中航向角估计在±π附近出现剧烈跳变滤波器发散。链路分析车辆的航向角本质上是周期性角度范围通常定义在[-π, π)或[0, 2π)。当真实航向角从π-0.01变化到-π0.01时数值上看起来像跳变了约2π。如果滤波器没有对角度做环绕处理计算innovation和差diff时会出现巨大误差。比如量测值是179度预测值是-179度误差不是-358度而是2度。如果滤波器直接把-358度当误差用状态修正会出错。这个问题的根源在于状态向量中的ψ是一个角度量任何涉及角度差的计算都要先做wrap到[-π, π)的处理。解决办法是写一个wrap_angle函数function wrapped wrap_angle(angle) wrapped atan2(sin(angle), cos(angle)); end在计算innovation时对角度量测对应的残差先做wrap在计算UKF的Sigma点加权协方差时对涉及角度的状态分量也要做环绕处理在计算EKF的预测差值时同样要注意角度分量的wrap。这个坑非常隐蔽因为不涉及角度的状态位置、速度一切正常只有航向角在特定角度处突然爆炸。如果你发现估计结果在车辆转圈时出现周期性发散90%的可能是角度环绕问题。6.4 排查链路三多传感器量测数据时间戳未对齐现象GPS、IMU和轮速的数据都正常但滤波器估计结果的误差比使用单一传感器还要大。链路分析多传感器融合的一个重要前提是各量测数据在时间上是对齐的。如果GPS数据滞后100ms而滤波器在用当前时刻的IMU数据和100ms之前的GPS数据同时更新相当于量测方程中存在不一致性滤波器会偏向滞后的GPS导致估计结果滞后且误差增大。Simulink中这个问题通常来自信号延迟模块设置不当或传感器的不同采样时间导致的固有延迟。Carsim与Simulink联合仿真时尤其容易出现因为Carsim的数据输出频率不稳定有时需要做缓冲对齐。如果条件允许最好的办法是在滤波器运行时同时记录每个量测的时间戳然后根据时间戳确定哪个数据可以用于当前时刻的量测更新。工程上简化的处理办法是以最高频率的IMU数据为主对低频GPS数据做最近邻匹配也就是在GPS数据到达时打一个标志位在下一拍滤波计算时使用最新的GPS数据。如果GPS和IMU的时间偏差超过一个GPS周期就需要考虑时间同步补偿算法了比如机械臂或无人机领域常用的视觉-IMU对齐方法。7. 从仿真到实测一次典型实验的完整流程与结果分析理论再扎实最终还是要走到实验这一步。我把自己做过的一组典型的车辆状态估计实验完整流程记录在这里。实验条件一辆搭载RTK-GPS、轮速传感器和IMU的测试车辆在一条包含直道和弯道的封闭测试道路上行驶。采集了真实轨迹作为ground truth然后对GPS量测加入2米标准差的高斯噪声对IMU加入0.1 rad/s的角速度噪声对轮速加入0.3 m/s的噪声。7.1 数据采集与时间同步处理采集过程中要注意几个细节轮速传感器通常输出的是脉冲信号解码后得到的是车辆纵向速度但不同车轮在转弯时速度不同。如果只使用单轮轮速在转弯时会产生偏差。更合理的做法是取后轮左右两个轮速的平均值或使用非驱动轮的轮速。另外IMU的安装位置和方向要标定准确特别是横摆角速度的方向如果安装反了估计结果会出现系统性偏差。时间同步方面我在采集时把每路数据都加上了一个时间戳。RTK-GPS的输出频率为10Hz轮速为50HzIMU为100Hz。在离线处理阶段我以IMU的时间为基准对GPS和轮速做线性插值对齐。这里需要注意的是插值会引入一定误差但相对于传感器本身噪声来说可以忽略。如果你在线实时处理建议用缓冲区方式等待GPS新数据到达后再进行一次量测更新不要强制把低频传感器数据插值到高频那会在实时系统中引入额外延迟。7.2 滤波性能指标与结果对比评估滤波器性能我一直使用两个基本指标RMSE均方根误差和最大绝对误差。分别对位置、速度、航向角三个核心状态计算。指标EKFUKF位置RMSE (m)1.871.54位置最大误差 (m)4.323.56速度RMSE (m/s)0.350.29航向角RMSE (rad)0.0740.052从这组结果看UKF在相同噪声条件下的位置RMSE比EKF低约17%航向角RMSE低近30%。这符合理论预期转弯工况中航向角的非线性最强UKF的二阶近似优势体现得最明显。真实工况与纯仿真在性能上还是有明显差异的。仿真中传感器噪声是理想高斯白噪声真实环境中的GPS多路径效应、轮速打滑、IMU温漂都会导致实际RMSE比仿真结果更大且可能出现连续数秒的误差高峰。我第一次把算法从仿真搬到实车数据上时位置误差直接比仿真翻了一倍后来仔细检查发现是轮速打滑导致的。直道上车辆急加速时驱动轮空转轮速传感器给出的速度远大于真实车速导致速度估计短暂偏差较大进而影响位置估计。解决思路是增大过程噪声中的σ_a值让滤波器不盲目相信模型预测同时可以把轮速量测的R设成动态值——检测到急加速时增大R的值降低轮速传感器在打滑工况下的可信度。这个做法本质上是一种简单的自适应噪声调整思想在很多场景下非常实用。8. 几个容易忽略的工程细节与参数设置建议最后这部分补充一些比较零碎但直接影响代码能否跑通或结果是否合理的经验点都是我实际动手过程中踩过的。第一P矩阵的初始化很关键不能随便设零矩阵。如果P_init全部为0滤波器从一开始就没有不确定性之后无论量测和过程怎么更新状态都不会改变多少。合理的初始P要反映你对初始状态的信任程度。如果你用第一个GPS位置和IMU数据来初始化状态位置方差直接用GPS精度即可速度可以用轮速精度航向角用IMU初始值精度。这样初始化之后滤波器一开始就处于较合理的置信区间。第二EKF的雅可比矩阵和UKF的Sigma点传播都要保证状态更新后的角度分量落在合理范围内否则会出现角度累积漂移。我在代码里习惯在每个滤波周期结束后对ψ做一次wrap处理。虽然滤波器内部计算用的是矩阵但状态量本身的角度属性需要显式处理不要等着它自己绕回来极容易绕成很大的角度影响后续的三角函数计算。第三Matlab代码里凡是涉及除法的地方都要检查分母为零的边界条件尤其是CTRV模型中 v/ψ_dot 这一项。如果你把ψ_dot的初始值设成0这个除法直接导致无穷大仿真必然发散。我开始用的是直接赋值后来改成阈值分支判断才把问题解决。这个阈值选多大也有讲究如果选得太大比如1e-2那么车辆在慢转弯时会被错误地当作直线运动处理位置估计会走直线与真实圆弧轨迹不符我实际测试下来 1e-6 到 1e-4 都是合理的范围建议取1e-6。第四Simulink仿真时间和步长的选择直接影响结果的数值稳定性。固定步长求解器建议选择ode4或ode5步长设为采样时间的最小公因数这样各模块的量测和状态更新才能严格对齐。如果你要用变步长务必设置最大步长限制否则Simulink可能会在状态变化平缓时使用很大的步长导致滤波器的量测更新不连续。另一个常被忽视的点是MATLAB Function块中如果使用randn生成量测噪声每次运行仿真会得到不同的噪声序列导致结果不可复现。建议设置随机数种子或者用一个单独的Constant模块噪声vector存储的方式把量测噪声固定下来这样调试时才能对比不同滤波器参数的实际差异。第五关于实时性优化。EKF的计算量集中在雅可比矩阵求值和矩阵乘法UKF的计算量集中在Sigma点传播和协方差加权。两者在Matlab环境下跑离线仿真没有任何性能压力但如果要移植到嵌入式平台建议把矩阵运算尽量改写成固定大小的运算避免动态内存分配。CTRV模型5维状态量对应5x5矩阵在C语言里用静态数组实现完全没有问题。UKF需要传播11个Sigma点2*51每个点都要调用一次过程模型和量测模型在MCU上可能会比较吃力这时可以尝试降维处理的传感器融合策略比如把位置和姿态分离成两个子滤波器而不是一个5维大滤波器。每次回头做这套流程我的体会是EKF和UKF的公式都不算复杂真正的难点永远在模型选择、噪声参数标定和工程细节处理上。如果你现在也在做行驶车辆状态估计希望这篇内容能帮你把前面的路看得更清楚。