forked from ZhanLi/UWBIns
29 lines
626 B
Mathematica
29 lines
626 B
Mathematica
|
function [Y] = fun1(X,dt)
|
|||
|
|
%FUN <EFBFBD>˴<EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD>йش˺<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ժҪ
|
|||
|
|
% <EFBFBD>˴<EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD>ϸ˵<EFBFBD><EFBFBD>
|
|||
|
|
WW = 0.05;
|
|||
|
|
nn = size(X,2);
|
|||
|
|
Y = zeros(5,nn);
|
|||
|
|
|
|||
|
|
for i=1:nn
|
|||
|
|
v = X(3,i);
|
|||
|
|
alfa = X(4,i);
|
|||
|
|
w = X(5,i)*180/pi;
|
|||
|
|
if w<WW
|
|||
|
|
alfa = alfa+w*dt;
|
|||
|
|
Y(:,i) = [X(1,i) + v*dt*cosd(alfa);
|
|||
|
|
X(2,i) - v*dt*sind(alfa);
|
|||
|
|
v;
|
|||
|
|
alfa;
|
|||
|
|
w*pi/180];
|
|||
|
|
else
|
|||
|
|
Y(:,i) = [X(1,i) + v/w*(sind(alfa+w*dt)-sind(alfa));
|
|||
|
|
X(2,i) - v/w*(cosd(alfa+w*dt)-cosd(alfa));
|
|||
|
|
v;
|
|||
|
|
alfa+w*dt;
|
|||
|
|
w*pi/180];
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
|