forked from ZhanLi/UWBIns
29 lines
626 B
Matlab
29 lines
626 B
Matlab
function [Y] = fun1(X,dt)
|
|
%FUN 此处显示有关此函数的摘要
|
|
% 此处显示详细说明
|
|
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
|
|
|