predict.m

来自「this code include particle filter techni」· M 代码 · 共 16 行

function xn=predict(xo,sxy,sv,maxX,maxY)
[n m]=size(xo);
A=[1 0 1 0;0 1 0 1;0 0 1 0;0 0 0 1];
x1=mvnrnd(0,sxy,n);
x2=mvnrnd(0,sxy,n);
x3=mvnrnd(0,sv,n);
x4=mvnrnd(0,sv,n);

xn=xo*A';

[r c] = find(xn(:,1)>0 | xn(:,1)<maxX);
xn(r,1) = xo(r,1);
[r c] = find(xn(:,2)>0 | xn(:,1)<maxY);
xn(r,2) = xo(r,2);

xn=xn + [x1 x2 x3 x4];