nextstate.m

来自「文件夹中NPFMain.m为滤波算法主运行程序」· M 代码 · 共 19 行

%NextState.m
%功能：由状态方程生成非机动时的下一时刻状态
function result=NextState(CurrentState)
global T;
global Phi;
global G;
global g;
global q;

w=zeros(4,1);
%%%产生协方差矩阵为Q的两组随机数
mat_conv=[T^(3/2)/sqrt(3) 0;sqrt(3)/2*sqrt(T) 0.5*sqrt(T)];
temp_rnd_xvx=normrnd(0,1,2,1);
temp_rnd_yvy=normrnd(0,1,2,1);
wx=mat_conv*temp_rnd_xvx;
wy=mat_conv*temp_rnd_yvy;
w=sqrt(q)*[wx;wy];
%%%产生随机数结束
result=Phi*CurrentState+G*DragAcce(CurrentState)+G*[0 -g]'+w;