EKF扩展卡尔曼滤波车身状态估计

基于EKF扩展卡尔曼滤波车身状态估计 汽车稳定性控制系统需要的状态信息一部分可通过车载传感器直接测量，另一部分不能直接测量。为了实现车辆动力学控制系统中的这种闭环状态反馈，受某些测量技术以及成本等因素的制约，依靠传感器直接测量获取某些重要状态量有很大的难度，因此提出状态估计的方法，即通过估计算法实时获取车辆在行驶过程中的某些重要状态量，如车速、横摆角速度、质心侧偏角等。本章利用扩展卡尔曼滤波方法，基于三自由度的车辆估计模型对轮边驱动电动汽车的纵向车速、侧向车速、质心侧偏角进行了估计，通过仿真验证了估计算法的准确性。

function X= EKF（u1u2u3u4u5u6u7u8u9u10u11u12）

a = 1.1597;
b = 1.215;
Iz = 1875;
m = 1620;
t = 0.002;
d=1.4;
N=4001;
vx0=80/3.6;vy0=0;wr0=0;
delta=u1;Fx_fl=u2;Fx_fr=u3;Fx_rl=u4;Fx_rr=u5;Fy_fl=u6;Fy_fr=u7;Fy_rl=u8;Fy_rr=u9;ax=u10;ay=u11;r=u12;
persistent P Q R P1 K xxhat xhat
if isempty（P）
   xhat = [vx0vy0wr0]‘;
   P =diag（[111]）; 
   Q =diag（[111]）;
   R =diag（[111]）;
end

for i=1:N
    
 vx_ekf=xhat（1i）;
 vy_ekf=xhat（2i）;
 wr_ekf=xhat（3i）;

    F_ekf=[0 wr_ekf vy_ekf;
           -wr_ekf 0 -vx_ekf;
           0 0 0];
    A_ekf=[0 wr_ekf/2 vy_ekf/2;
           -wr_ekf/2 0 -vx_ekf/2;
           0 0 0];
    C_ekf=[（（Fx_fl+Fx_fr）*cos（delta）+Fx_rl+Fx_rr-（Fy_fl+Fy_fr）*sin（delta））/m;
           （