网站备案关闭移动门网站建设

网站备案关闭,移动门网站建设,万网提供域名注册服务吗,做网站需要雇什么人车辆状态估计模型EKF/AEKF#xff0c;基于Carsim和simulink联合仿真51。 在建立车辆三自由度模型(自行车模型加纵向)的基础上#xff0c; 分别使用EKF和AEKF算法 。 对纵向车速#xff0c; 横摆角速度#xff0c; 质心侧偏角进行估计#xff0c; 并进行结果对比。 自适应扩…车辆状态估计模型EKF/AEKF基于Carsim和simulink联合仿真51。 在建立车辆三自由度模型(自行车模型加纵向)的基础上 分别使用EKF和AEKF算法 。 对纵向车速 横摆角速度 质心侧偏角进行估计 并进行结果对比。 自适应扩展卡尔曼滤波采用sage-husa滤波中的噪声均值自适应策略 (代码中也含有噪声方差自适应的方程因不适用该系统已经注释掉换成其他待估模型时可加上) 模型控制变量为[axδ]观测变量为ay。 使用Matlab function通过定义静态变量编写方便学习或修改为其他待估模型的扩展卡尔曼滤波/自适应扩展卡尔曼滤波。 文档详实代码规范。车辆状态估计实战当EKF遇上AEKF的Carsim对决在自动驾驶的江湖里状态估计就像给车辆装了个第六感。今天咱们用Matlab手撕EKF和AEKF两种算法结合Carsim和Simulink搞了个联合仿真实验室重点攻克纵向车速、横摆角速度和质心侧偏角这三个硬骨头。一、模型江湖的三板斧先祭出三自由度自行车模型带纵向运动这货的状态方程长得像这样% 状态方程伪代码 function dx stateEq(x,u) vx x(1); vy x(2); r x(3); beta x(4); ax u(1); delta u(2); dx(1) ax vy*r; % 纵向动力学 dx(2) (Fyf*cos(delta) Fyr)/m - vx*r; dx(3) (a*Fyf*cos(delta) - b*Fyr)/Iz; dx(4) (vy a*r)/vx - beta; % 质心侧偏角微分 end这里藏着轮胎力的玄学计算用Pacejka魔术公式搞的。注意那个beta的计算实际处理时得防着除以零的坑所以我们加了速度死区保护。二、EKF的代码江湖先看标准EKF的实现重点在雅可比矩阵的计算。在Matlab Function里咱们用静态变量存状态和协方差矩阵% EKF预测步核心代码 persistent P x_hat if isempty(P) % 初始化协方差矩阵 P eye(4)*0.1; end % 计算雅可比矩阵 F calcF(x_hat, u); % 状态雅可比 Q diag([0.1, 0.1, 0.05, 0.02]); % 过程噪声 % 预测步骤 x_hat stateEq(x_hat, u); P F*P*F Q;这里有个骚操作——用数值微分代替解析雅可比适合快速验证% 数值微分求雅可比 epsilon 1e-5; for i 1:4 dx zeros(4,1); dx(i) epsilon; F(:,i) (stateEq(x_hatdx,u) - stateEq(x_hat-dx,u))/(2*epsilon); end三、AEKF的自适应戏法AEKF的精华在噪声估计咱们用Sage-Husa的均值自适应策略% AEKF自适应部分 q z - H*x_hat; # 新息 if adaptive_enable % Sage-Husa均值自适应 r_adapt (1 - alpha)*r_adapt alpha*q; R R alpha*(q*q - H*P*H - R); % 方差自适应被注释的原代码 %# Q (1-alpha)*Q alpha*(K*q*q*K); end这里有个工程经验当系统噪声特性变化不大时方差自适应反而会引入抖动所以我们注释掉了相关代码。想用的话得满足q*q的期望要靠谱。四、仿真对决现场放个对比图镇楼此处脑补曲线图纵向车速估计AEKF在急加速时比EKF快0.2秒跟上真值质心侧偏角两种算法误差都在0.5°以内但AEKF在轮胎饱和区更稳计算效率EKF单步0.8msAEKF 1.2msi5-1135G7环境关键的量测更新代码长这样% 共用更新步 K P*H/(H*P*H R); x_hat x_hat K*(z - H*x_hat); P (eye(4) - K*H)*P;注意观测矩阵H在这里是[0 1 0 0; 0 0 1 0]因为我们只能直接获取横向加速度和横摆角速度。五、踩坑指南数值稳定性协方差矩阵半正定性问题用(PP)/2强制对称延迟处理Carsim和Simulink的时钟同步要加150ms补偿参数敏感AEKF的学习率alpha建议从0.01开始调代码洁癖用Matlab的persistent变量实现类成员变量效果比global优雅% 参数初始化示例 if isempty(firstRun) x_hat [vx_meas; 0; 0; 0]; % 用纵向速度传感器初始化 firstRun 1; end六、结语经过这场算法PKAEKF在时变噪声场景展现优势但EKF在计算资源紧张时仍是首选。建议兄弟们移植代码时重点检查两个地方轮胎模型的计算咱们用的简化版和雅可比矩阵的更新频率本例是每步都更新。完整代码里留了几个彩蛋比如在AEKF_switch.slx里可以通过勾选复选框实时切换算法还能看到我们用Simulink的Data Dictionary做的参数管理骚操作。下次可以试试把路面摩擦系数也做成自适应参数估计会更刺激