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

function X= EKF(u1,u2,u3,u4,u5,u6,u7,u8,u9,u10,u11,u12) 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 = [vx0,vy0,wr0]'; P =diag([1,1,1]); Q =diag([1,1,1]); R =diag([1,1,1]); end for i=1:N vx_ekf=xhat(1,i); vy_ekf=xhat(2,i); wr_ekf=xhat(3,i); 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; ((Fx_fl+Fx_fr)*sin(delta)+Fy_rl+Fy_rr+(Fy_fl+Fy_fr)*cos(delta))/m; (a*((Fx_fl+Fx_fr)*sin(delta)+(Fy_fl+Fy_fr)*cos(delta))-b*(Fy_rl+Fy_rr)+((Fx_fr-Fx_fl)*cos(delta)+(Fy_fl-Fy_fr)*sin(delta)+(Fx_rr-Fx_rl))*d/2)/Iz]; D_ekf=[0 0 0 0 0 0 0 0 1]; fai_ekf=eye(3)+A_ekf*t; CC_ekf=C_ekf*t; FF_ekf=eye(3)+F_ekf*t; xxhat(:,i)=fai_ekf*xhat(:,i)+CC_ekf; P1(:,:,i)=FF_ekf*P(:,:,i)*(FF_ekf)'+Q; K(:,:,i)=P1(:,:,i)*(D_ekf)'*inv(D_ekf*P1(:,:,i)*(D_ekf)'+R); xhat1(:,i)=xxhat(:,i)+K(:,:,i)*([ax;ay;r]-D_ekf*xxhat(:,i)); P(:,:,i+1)=(eye(3)-K(:,:,i)*D_ekf)*P1(:,:,i); xhat(:,:,i+1)=xhat1(:,i); X=xhat1(:,i); end