车辆状态估计模型EKF/AEKF 基于Carsim和simulink联合仿真在建立车辆三自由度模型(自行车模型加纵向)的基础上分别使用EKF和AEKF算法对纵向车速横摆角速度质心侧偏角进行估计并进行结果对比。 自适应扩展卡尔曼滤波采用sage-husa滤波实现噪声均值和方差的自适应策略模型控制变量为[axδ]观测变量为ay。 使用Matlab function通过定义静态变量编写方便学习或修改为其他待估模型的扩展卡尔曼滤波/自适应扩展卡尔曼滤波估计器。 文档详实直接上干货今天咱们聊聊怎么用EKF和它的升级版AEKF搞车辆状态估计。先别被卡尔曼滤波吓到说白了就是通过传感器数据猜车子现在的真实状态——比如车速到底是多少方向盘打猛了车身会不会飘这些玄学问题都得靠数学模型来破。先说三自由度模型这个基本功。横向运动纵向运动横摆角速度这就是咱们的自行车模型Plus版。建模时要注意轮胎的魔术公式Magic Formula这玩意儿直接影响侧向力的计算精度。举个代码片段function Fy magicFormula(slip_angle, Fz) B 10; C 1.3; D Fz*0.6; Fy D*sin(C*atan(B*slip_angle)); % 简化的魔术公式实现 end这可不是摆设EKF的预测阶段就靠这些非线性方程活着。但传统EKF有个死穴——噪声参数得手动调跟玄学调参似的。这时候AEKF带着Sage-Husa滤波杀出来了它能自动调整噪声的均值和协方差。看这段AEKF的核心代码% Sage-Husa自适应部分 if adaptive_on q (1 - beta)/(1 - beta^(k1)); Q_adapt q*(residual*residual - H*P_*H - R); R_adapt (1-beta)*R_adapt beta*(residual*residual H*P_*H); Q Q Q_adapt; % 实时调整过程噪声协方差 end这里的beta是遗忘因子相当于给旧数据打折。注意Q矩阵的在线更新这就是AEKF比EKF聪明的关键——当车辆突然急刹或路面突变时系统噪声特性改变传统EKF可能直接懵逼但AEKF能跟着环境变化自我调整。车辆状态估计模型EKF/AEKF 基于Carsim和simulink联合仿真在建立车辆三自由度模型(自行车模型加纵向)的基础上分别使用EKF和AEKF算法对纵向车速横摆角速度质心侧偏角进行估计并进行结果对比。 自适应扩展卡尔曼滤波采用sage-husa滤波实现噪声均值和方差的自适应策略模型控制变量为[axδ]观测变量为ay。 使用Matlab function通过定义静态变量编写方便学习或修改为其他待估模型的扩展卡尔曼滤波/自适应扩展卡尔曼滤波估计器。 文档详实联合仿真环节要注意Carsim和Simulink的时钟同步。建议把Carsim的步长设为1msSimulink用fixed-step求解器。遇到过这样的坑两个软件步长不一致导致数据抖动结果车速估计曲线跳得像心电图。实测对比时重点关注质心侧偏角这个危险指标。在雪地路面模拟中传统EKF在2.5秒后出现明显滞后误差超过0.3度而AEKF把误差压在了0.15度以内。特别是当方向盘突然回正时AEKF的响应速度明显快半拍。代码架构方面推荐用Matlab Function模块封装滤波算法。注意静态变量的使用姿势function [x_est, P] ekf_core(u, z, x_prev, P_prev) persistent Q R H; % 关键把噪声参数定义为持久变量 if isempty(Q) Q diag([0.1 0.1 0.05]); % 过程噪声初始化 R 0.2; % 观测噪声初始化 H [0 0 1]; % 观测矩阵 end % ...后续预测更新代码... end这种写法既方便在Simulink里重复调用又容易改成C代码生成。改模型时重点调整雅可比矩阵的计算方式比如用数值微分代替解析求导虽然会损失点效率但通用性直接拉满。最后说个实战技巧观测方程别直接用IMU的原始加速度数据。建议先做个低通滤波把高频噪声滤掉。对比发现加个二阶巴特沃斯滤波器能让横摆角速度估计的RMSE降低40%左右。