粒子滤波CARSIM和matlab联合仿真

Question

我在用carsim和simulink联合仿真时出现了一些问题，求指点

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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;        %粒子集合              
%z=[z,u(1:2,:)];                    

% 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=100;
N=200;

% 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;                        
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

**运行仿真时出错，仿真终止原因: 在时间 0.0，flag = 2 (update) 时执行 MATLAB S-Function 'APF' 时 'ipf/S-Function' 中出错。 Index in position 2 exceeds array bounds. Index must not exceed 1.

我把粒子数改成1就可以超过1就报错

Answer 1

• 这篇文章讲的很详细，请看：关于Matlab和Carsim联合仿真问题