UKF.rar_SIMULINK_SIMULINK ukf_simulink UKF_汽车 状态估计_汽车状态

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [sys,x0,str,ts]=UKFF(t,x,u,flag) global Zdist; %观测信息 global Xukf; %粒子滤波状态估计 Q=diag([1,10,1,10]); %过程噪声Q R=100; %测量噪声R switch flag case 0 %系统进行初始化，调用mdlInitializeSizes函数 [sys,x0,str,ts]=mdlInitializeSizes; case 2 %更新离散状态变量，调用mdlUpdate函数 sys=mdlUpdate(t,x,u,Q,R); case 3 %计算S函数的输出，调用mdlOutputs函数 sys=mdlOutputs(t,x,u); case {1,4} sys=[]; case 9 %仿真结束，保存状态值 save('Xukf','Xukf'); save('Zdist','Zdist'); otherwise %其他未知情况处理，用户可以自定义 error(['Unhandled flag = ',num2str(flag)]); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %1.系统初始化子函数 function [sys,x0,str,ts]=mdlInitializeSizes sizes = simsizes; sizes.NumContStates = 0; sizes.NumDiscStates = 4; sizes.NumOutputs = 4; sizes.NumInputs = 2; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0 = [0,0,0,0]'; %初始条件 str = []; ts = [-1 0];%表示该模块采样时间继承其前的模块采样时间设置 global Zdist; %观测信息 Zdist=[]; global Xukf; %粒子滤波估计状态 Xukf=[x0]; global P; P=eye(4)*1000; %初始化 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %2.进行离散状态变量的更新 function sys=mdlUpdate(t,x,u,Q,R) global Zdist;%观测信息 global Xukf;%例子滤波状态估计 global P;%全局变量P Zdist=[Zdist,u]; m=25000;%设定整车质量 g=9.8;%重力加速度 Hrc=0.24;%设定车辆的质心高度 ht=1; k=400000;%设定车辆的悬架侧倾刚度 c=487050;%设定车辆的悬架阻尼系数 C=[0 0 c/m/(Hrc+ht)/2 (k-2*m*g*ht)/m/(Hrc+ht)/2];%设置输出矩阵 %—————————————————————————————————————— %下面开始用UKF对状态更新 Xin=Xukf(:,length(Xukf(1,:))); Zdist=[Zdist,C*Xin]; %%%%为什么不是u？ [Xnew,P]=GetUkfResult(Xin,u,P,Q,R) Xukf=[Xukf,Xnew]; sys=Xnew; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function sys=mdlOutputs(t,x,u) sys = x; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [Xout,P]=GetUkfResult(Xin,u,P,Q,R) %%%% u或者z？ L=4; alpha=0.01; kalpha=0; belta=2; ramda=alpha^2*(L+kalpha)-L; for j=1:2*L+1 Wm(j)=1/(2*(L+ramda)); Wc(j)=1/(2*(L+ramda)); end Wm(1)=ramda/(L+ramda); Wc(1)=ramda/(L+ramda)+1-alpha^2+belta; xestimate=Xin; P cho=(chol(P*(L+ramda)))'; for k=1:L xgamaP1(:,k)=xestimate+cho(:,k); xgamaP2(:,k)=xestimate-cho(:,k); end Xsigma=[xestimate,xgamaP1,xgamaP2]; %Sigma点集 for i=1:2*L+1 % i或者k？ m=25000;%设定整车质量 g=9.8;%重力加速度 Hrc=0.24;%设定车辆的质心高度 ht=1; jxx=7965;%设定车质量绕车身侧倾轴的转动惯量 jx=jxx+m*ht^2;%设定车质量绕车身质心纵轴的转动惯量 jz=86300;%设定车辆质心绕Z轴的转动惯量 k=400000;%设定车辆的悬架侧倾刚度 c=487050;%设定车辆的悬架阻尼系数 a=3.64;%设定车辆质心到前轴的距离 f=3.36;%设定车辆质心到后轴的距离 b=1.3;%设定车辆中后轴的距离 kf=227300;%设定前轮等效侧偏刚度 km=487050;%设定中轮等效侧偏刚度 kr=487050;%设定后轮等效侧偏刚度 p1=kf+kr+km;%设定辅助变量 q1=km*(f-b)+kr*f-kf*a;%设定辅助变量 t1=km*(f-b)^2+kr*f^2+kf*a^2;%设定辅助变量 A=[-p1/(m*u(1)) q1/(m*u(1)^2)-1 0 0; q1/jz -t1/(jz*u(1)) 0 0; -p1*(Hrc+ht)/jx q1*(Hrc+ht)/(jx*u(1)) -c/jx (m*g*ht-k)/jx; 0 0 1 0]; B=[kf/(m*u(1));kf*a/jz;(Hrc+ht)*kf/jx;0]; dt=0.01; Xsigmapre(:,i)=(A*dt+eye(4))*Xsigma(:,i)+B*u(2)*dt+0.5*A*dt^2*B*u(2); C=[0 0 c/m/(Hrc+ht)/2 (k-2*m*g*ht)/m/(Hrc+ht)/2];%设置输出矩阵 Zsigmapre(1,i)=C*Xsigmapre(:,i); end Xpred=zeros(4,1); for i=1:2*L+1 Xpred=Xpred+Wm(i)*Xsigmapre(:,i); end Ppred=zeros(4,4); %协方差阵预测 for i=1:2*L+1 Ppred=Ppred+Wc(i)*(Xsigmapre(:,i)-Xpred)*(Xsigmapre(:,i)-Xpred)'; end Ppred=Ppred+Q; chor=(chol((L+ramda)*Ppred))'; for i=1:L XaugsigmaP1(:,i)=Xpred+chor(:,i); XaugsigmaP2(:,i)=Xpred-chor(:,i); end Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2]; %第五步：观测预测 %第六步：计算观测预测均值和协方差 Zpred=0; for k=1:2*L+1 Zpred=Zpred+Wm(k)*Zsigmapre(k);%%式子有不同 end Pzz=0; for k=1:2*L+1 Pzz=Pzz+Wc(k)*(Zsigmapre(k)-Zpred)*(Zsigmapre(1,k)-Zpred)'; end Pzz=Pzz+R; Pxz=zeros(4,1); for k=1:2*L+1 Pxz=Pxz+Wc(k)*(Xaugsigma(:,k)-Xpred)*(Zsigmapre(1,k)-Zpred)'; end m=25000;%设定整车质量 g=9.8;%重力加速度 Hrc=0.24;%设定车辆的质心高度 ht=1; jxx=7965;%设定车质量绕车身侧倾轴的转动惯量 jx=jxx+m*ht^2;%设定车质量绕车身质心纵轴的转动惯量 jz=86300;%设定车辆质心绕Z轴的转动惯量 k=400000;%设定车辆的悬架侧倾刚度 c=487050;%设定车辆的悬架阻尼系数 a=3.64;%设定车辆质心到前轴的距离 f=3.36;%设定车辆质心到后轴的距离 b=1.3;%设定车辆中后轴的距离 kf=227300;%设定前轮等效侧偏刚度 km=487050;%设定中轮等效侧偏刚度 kr=487050;%设定后轮等效侧偏刚度 p1=kf+kr+km;%设定辅助变量 q1=km*(f-b)+kr*f-kf*a;%设定辅助变量 t1=km*(f-b)^2+kr*f^2+kf*a^2;%设定辅助变量 A=[-p1/(m*u(1)) q1/(m*u(1)^2)-1 0 0; q1/jz -t1/(jz*u(1)) 0 0; -p1*(Hrc+ht)/jx q1*(Hrc+ht)/(jx*u(1)) -c/jx (m*g*ht-k)/jx; 0 0 1 0]; B=[kf/(m*u(1));kf*a/jz;(Hrc+ht)*kf/jx;0]; dt=0.01; XX=(A*dt+eye(4))*Xin+B*u(2)*dt+0.5*A*dt^2*B*u(2); CC=[0 0 c/m/(Hrc+ht)/2 (k-2*m*g*ht)/m/(Hrc+ht)/2];%设置输出矩阵 K=Pxz*inv(Pzz);%Kalman增益 Xout=Xpred+K*(CC*XX-Zpred);%状态更新 P=Ppred-K*Pzz*K';%方差更新 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%