%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [sys,x0,str,ts]=APF(t,x,u,flag)
global Zdist; %观测信息
global Xpf; %粒子滤波估计状态
global Xpfset; %粒子集合
randn('seed',20);
N=200; %粒子数目
%粒子滤波"网"的半径，衡量粒子集合的分散度，相当于过程噪声Q
% NETQ=diag([0.0001,0.0009]);
%
% NETR=0.01;
switch flag
case 0 %系统初始化，调用mdlInitializeSizes函数
[sys,x0,str,ts]=mdlInitializeSizes(N);
case 2 %更新离散状态变量，调用mdlUpdate函数
sys=mdlUpdate(t,x,u);
case 3 %计算S函数的输出，调用mdlOutputs
sys=mdlOutputs(t,x,u);
case {1,4}
sys=[];
case 9 %仿真结束，保存状态值
save('Xpf','Xpf');
save('Zdist','Zdist');
otherwise %其他情况处理，用户自定义
error(['Unhandled flag = ',num2str(flag)]);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%1、系统初始化子函数
function [sys,x0,str,ts]=mdlInitializeSizes(N)
sizes = simsizes;
sizes.NumContStates = 0; %无连续量
sizes.NumDiscStates = 2; %离散状态4维
sizes.NumOutputs = 2; %输出4维
sizes.NumInputs = 3; %输入维数
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1; %至少需要的采样时间
sys = simsizes(sizes);
x0 = [0.01;0.01]; %初始条件
str = []; %str总是设置为空
ts = [0.01 0]; %表示改模块采样时间继承其前的模块采样时间设置
%粒子集合初始化
global Xpfset;
Xpfset=zeros(2,N);
global Zdist; %观测信息
Zdist=[];
global Xpf; %粒子滤波估计状态
P=1;
for i=1:N
Xpf(:,i)=x0+sqrt(P)*randn(2,1);
end
% for i=1:M
% xpart(:,i)=x+sqrt(P)*randn(2,1);
% end
Xpf=x0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%2、进行离散状态变量更新
function sys=mdlUpdate(t,x,u,N)
global z; %观测信息
global Xpf;
global Xpfset; %粒子集合
% x=[0;0];%初始状态
% T=0.01;%离散时间
m=1620;%总质量
Iz=1875;%横摆转动惯量
Cf=-1e5;%前轮侧偏刚度
Cr=-1.1e5;%后轮侧偏刚度
lf=1.1597;%前轴距质心距离
lr=1.215;%后轴距质心距离
a=Cf+Cr;
b=Cf*lf-Cr*lr;
c=Cf*lf^2+Cr*lr^2;
a1=0.1^2;
a2=0.01^2;
a3=0.1^2;
a4=0.01^2;
M=400;
N=400;
% xpartminus=zeros(2,M);
% zpart=zeros(2,M);
w=zeros(1,N);
%x=zeros(2,N);
T=1;
vx=60/3.6;
SW=u(1);
yaw=u(2);
ay=u(3);
% A=[a*T/(m*vx)+1,(b/(m*vx^2)-1)*T;b*T/Iz,c*T/(Iz*vx)+1];
% B=[-Cf*T/(m*vx);-Cf*lf*T/Iz];
% C=[a/m,b/(m*vx);0,1];
% D=[-Cf/m;0];
%
% z=[ay+sqrt(a3)*randn(1,1);yaw+sqrt(a4)*randn(1,1)];
for k=1:M
A=[a*T/(m*vx(k))+1,(b/(m*vx(k)^2)-1)*T;b*T/Iz,c*T/(Iz*vx(k))+1];
B=[-Cf*T/(m*vx(k));-Cf*lf*T/Iz];
C=[a/m,b/(m*vx(k));0,1];
D=[-Cf/m;0];
% u=delta;
z=[ay(k)+sqrt(a3)*randn(1,1);yaw(k)+sqrt(a4)*randn(1,1)];
for i=1:N
Xpfset(:,i)=A*Xpf(:,i)+B*SW(k)+[sqrt(a1)*randn(1,1);sqrt(a2)*randn(1,1)];
zpart=C*Xpfset(:,i)+D*SW(k);
v=norm(z-zpart);
w(i)=(1/sqrt(a3)/sqrt(2*pi))*exp(-v^2/2/a3);
qsum=sum(w);
w(i) = w(i)/qsum;
SW=rand;
qtempsum=0;
for j=1:N
qtempsum=qtempsum+w(j);
if qtempsum>=SW
Xpf(:,i)=Xpfset(:,j);
break
end
end
end
end
Xnew(:,k)=[mean(Xpfset(1,:)),mean(Xpfset(2,:))]';
Xpf=[Xpf,Xnew];
sys=Xnew;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function sys=mdlOutputs(~,x,~)
sys = x;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
