Simulink和Carsim联合仿真车辆状态估计方法:卡尔曼滤波的EKF，UKF，CKF等...

AUVpwoxkW

786人浏览 · 2026-01-26 13:28:05

AUVpwoxkW · 2026-01-26 13:28:05 发布

Simulink和Carsim联合仿真车辆状态估计方法:卡尔曼滤波的EKF，UKF，CKF等系统的输入量:前轮转角，纵向加速度，侧向加速度等(可以自由定义) 估计的状态量:横摆角速度，质心侧偏角，纵向车速，侧向车速 Carsim提供实车的数据，Simukink中搭建观测器，S函数写的滤波代码，易懂易调，联合仿真相比于纯Simulink模型的状态估计，联合仿真更具有说服力，所有模型均为本人硕士期间搭建，有完整的模型，注释，参数，说明文档，可提供各种细节，支持一手资料

直接上干货。搞车辆状态估计的老铁应该都清楚，联合仿真这玩意儿有多香——Carsim里跑着带魔术轮胎的八自由度整车模型，Simulink这边怼个自己写的状态观测器，左手实车数据右手估计算法，调参的时候不用对着仿真模型自嗨，这波操作稳得一批。

咱今天重点唠唠怎么用S函数手撕卡尔曼全家桶。先看输入输出：前轮转角直接从Carsim的方向盘转角换算，纵向加速度用IMU信号加个滑动窗口滤波，侧向加速度记得做坐标变换。要估的四个状态量里，质心侧偏角这货最难搞，毕竟没法直接测量，得靠横摆角速度和车速反推。

以UKF为例，核心代码就三块：

function [x_est, P] = UKF_predict(f, x, P, Q, dt)
    % 生成Sigma点
    [Xsigma, W] = scaled_sigma_points(x, P, alpha, beta, kappa);
    
    % 传播Sigma点
    Xpred = zeros(size(Xsigma));
    for i = 1:size(Xsigma,2)
        Xpred(:,i) = f(Xsigma(:,i), dt);  % 非线性状态方程
    end
    
    % 计算预测均值和协方差
    x_pred = Xpred * W(:);
    P_pred = Q;
    for i = 1:size(Xsigma,2)
        P_pred = P_pred + W(i)*(Xpred(:,i)-x_pred)*(Xpred(:,i)-x_pred)';
    end
end

这段预测步的关键在于sigma点的权重计算。参数alpha控制分布范围，beta针对非高斯特性优化，实测时beta=2对车辆模型最友好。注意动力学模型f()要单独封装，建议用.m文件别整匿名函数，方便调试。

观测更新部分有个坑：Carsim输出的横摆角速度自带高频噪声，得在观测方程里加低通滤波。我的处理方案是在S函数里嵌套了个二阶Butterworth：

function y = measurement_model(x)
    persistent prev_yaw_rate;
    if isempty(prev_yaw_rate)
        prev_yaw_rate = x(1);
    end
    
    % 二阶巴特沃斯滤波
    [b,a] = butter(2, 10/(1000/2));  % 10Hz截止频率
    current_yaw = filter(b, a, x(1)-prev_yaw_rate);
    prev_yaw_rate = x(1);
    
    y = [current_yaw; 
         x(2); 
         x(3)*cos(x(4)) - x(5)*sin(x(4));  % 纵向车速
         x(3)*sin(x(4)) + x(5)*cos(x(4))]; % 侧向车速
end

滤波截止频率别拍脑袋定，建议先跑个Carsim工况，把原始信号频谱分析一波。实测发现当车速超过80km/h时，横摆角频率成分主要分布在0-5Hz，这时候截止频率设10Hz能有效去毛刺又不损失动态。

调参玄学时间到！Q矩阵和R矩阵的初始化千万别信教科书上的鬼话。我的祖传秘方是：先让Q=diag([0.1, 0.5, 0.3, 0.2])，R=diag([0.05, 0.1, 0.2, 0.2])，然后开着仿真边跑边调。重点看质心侧偏角的收敛速度——出现超调就加大Q(2,2)，响应慢了就减小R(4,4)。