matlab程序跑不出结果

Question

matlab在运行crossing_strait中的tau12时会卡住，真心请教！
之前能够运行，但是没保存那个版本，新写的在tau12处一直跑不出结果
tau12涉及的是矩阵的各种运算

USV主程序：
clear

% 定义状态变量% 定义Table2参数
params.k11 = 1;
params.k12 = 1;
params.k13 = 1;
params.ke = 50;
params.s1 = -10;
params.s2 = -0.05;
params.C1 = -1;
params.s3 = 10;
params.s4 = -0.05;
params.C2 = 1;
params.t1 = 12.26;
params.tf = 58;
params.th=58;
params.td=80;    %暂定的仿真结束时间
params.k21 = 10;
params.k23 = 10;
params.ki = 100;
params.d21 = -1;
params.d22 = -1;
params.c1 = 10;
params.c2 = 1;
params.c3 = 1;

% 定义Table1参数
params.m_n = 23.8;
params.Xu_n = -0.72253;
params.Nv_n = 0.0313;
params.Yv1_n = -10;
params.Ngamma1_n = -1;
params.Iz_n = 1.76;
params.Yv_n = -0.88965;
params.Ngamma_n = -1.9;
params.Ygamma1_n = -0.1;
params.xg_n = 0.046;
params.Ygamma_n = -7.25;
params.Xu1_n = -2;
params.Nv1_n = 0;

 %定义 假设5中的参数（tau31）
params.alpha1 = 1; %暂定
params.alpha2 = 2; 
params.alpha3 = 3; 
params.alphatemp = [params.alpha1,params.alpha2,params.alpha3];
params.alpha = max(params.alphatemp);


%初始状态
xu = 0; yv = 0; psigamma = 0;
x = 0; y = 0; psi = 0;
u = 5; v = 0; gamma = 1;

%初始状态(body-fixed) 用于ode函数
initial_conditions1 = [xu, yv, psigamma, u, v, gamma];%uvgamma

%仿真时间范围（圆形轨迹侦察阶段）
tspan1 = [0 params.t1]; % t1 = 12.26
tspan2 = [params.t1 params.td]; 

 %使用ode45进行仿真无人侦察阶段
[T1, Y1] = ode45(@(t, state) reconnaissance(t, state, params),tspan1, initial_conditions1);

initial_conditions2 = Y1(end, :);%更新初始条件为侦察阶段结束时的状态

[T2, Y2] = ode45(@(t, state) crossing_strait(t, state, params), tspan2, initial_conditions2);

crossing_strait函数
function dstate = crossing_strait (t,state,params)

xu = state(1);
yv = state(2);
psigamma = state(3);
u = state(4);
v = state(5);
gamma = state(6);


theta = [u,v,gamma]';%下文矩阵用使用

%读取参数
m_n = params.m_n;
Xu_n = params.Xu_n;
Nv_n = params.Nv_n;
Yv1_n = params.Yv1_n;
Ngamma1_n = params.Ngamma1_n;
Iz_n = params.Iz_n;
Yv_n = params.Yv_n;
Ngamma_n = params.Ngamma_n;
Ygamma1_n = params.Ygamma1_n;
xg_n = params.xg_n;
Ygamma_n = params.Ygamma_n;
Xu1_n = params.Xu1_n;
Nv1_n = params.Nv1_n;

m = m_n+0.2*m_n*sin(t);
Xu = Xu_n+0.2*Xu_n*sin(t);
Nv = Nv_n+0.2*Nv_n*sin(t);
Yv1 = Yv1_n+0.2*Yv1_n*sin(t);
Ngamma1 = Ngamma1_n+0.2*Ngamma1_n*sin(t);
Iz = Iz_n+0.2*Iz_n*sin(t);
Yv = Yv_n+0.2*Yv_n*sin(t);
Ngamma = Ngamma_n+0.2*Ngamma_n*sin(t);
Ygamma1 = Ygamma1_n+0.2*Ygamma1_n*sin(t);
xg = xg_n+0.2*xg_n*sin(t);
Ygamma = Ygamma_n+0.2*Ygamma_n*sin(t);
Xu1 = Xu1_n+0.2*Xu1_n*sin(t);
Nv1 = Nv1_n+0.2*Nv1_n*sin(t);




%define matrix parameter
C13 = -(m-Yv1)*v-(1/2)*(m*xg-Ygamma1+m*xg-Nv1)*gamma;
C13_n = -(m_n-Yv1_n)*v-(1/2)*(m_n*xg_n-Ygamma1_n+m_n*xg_n-Nv1_n)*gamma;
C23 = -(m-Xu1)*u;
C23_n = -(m_n-Xu1_n)*u;

R=[1 0 0;0 1 0;0 0 1];%此阶段的旋转矩阵为单位阵
M=[m-Xu1 0 0;0 m-Yv1 m*xg-Ygamma1;0 m*xg-Nv1 Iz-Ngamma1];%惯性质量矩阵
M_n=[m_n-Xu1_n 0 0;0 m_n-Yv1_n m_n*xg_n-Ygamma1_n;0 m_n*xg_n-Nv1_n Iz_n-Ngamma1_n];
C=[0 0 C13;0 0 C23;-C13 -C23 0];%Coriolis matrix
C_n = [0 0 C13_n;0 0 C23_n;-1*C13_n -1*C23_n 0];
D=[-Xu 0 0;0 -Yv -Ygamma;0 -Nv -Ngamma];%hydrodynamic damping matrix
D_n = [-Xu_n 0 0;0 -Yv_n -Ygamma_n;0 -Nv_n -Ngamma_n];

dxu = u;
dyv = v;
dpsigamma = gamma;



%约束方程
Ai = [params.k21 0 0;0 0 params.k23];                 %2*3
ci = [-(xu-t)/params.k21+1;-psigamma/params.k23];     %2*1
bi = [-(dxu-1)/params.k21;-dpsigamma/params.k23];     %2*1

Bi = Ai*Ai';                                          %2*2可逆
betai = Ai*theta-ci;                                   %2*1
Piei = [norm(theta)^2;norm(theta);1 ];                %3*1 未知
Gamma = eye(2);                                       %暂定 此处修改为2*2

tau12_temp1=(bi+Ai/M_n*(C_n*theta+D_n*theta));
tau12_temp2=sqrtm(M_n);
tau12 = tau12_temp2*pinv(Ai/tau12_temp2)*tau12_temp1; %3*1
tau22 = -params.ki*M_n*Ai'/Bi/Gamma*betai;                                 %3*1
tau32 =-1*(M_n*Ai'/Bi/Gamma)*betai*norm(Piei)^2;                        %3*1                      %3*1

%定义约束力
Qe2 =tau12+tau22+tau32;

dstate_temp2= inv(M)*(-C*theta-D*theta+Qe2);  %2*1;
du = dstate_temp2(1);
dv = 0;
dgamma = dstate_temp2(2);

dstate = [dxu;dyv;psigamma;du;dv;dgamma];

end