采用龙格库塔解微分方程时，在运行一段时间后，输出变为1.#QNAN

羽剑 · 发表于 2016-4-29 15:27:07

求助：在使用龙格库塔法解微分方程的程序是，程序总会在运行一段时间后，输出的结果变为1.#QNAN，中间的数据也全部变为1.#QNAN。代码如下：（ps ph th为最后得到的结果）

float theta=0, phi=0, psi=0;
float ps, ph, th;
float ps_old=0, ph_old=0, th_old=0,
ps_d, ph_d, th_d,cj;
float time=0.1;

float a11=1,a12=0,a13=0,a21=0,a22=1,a23=0,a31=0,a32=0,a33=1;
float b11=0,b12=0,b13=0,b21=0,b22=0,b23=0,b31=0,b32=0,b33=0;
float u1,u2,u3;

void Solve(float w_x,float w_y,float w_z)
{

float F_a11, F_a12,F_a13,F_a21, F_a22,F_a23,F_a31, F_a32,F_a33,
k1_11,k1_12,k1_13,k1_21,k1_22,k1_23,k1_31,k1_32,k1_33,
k2_11,k2_12,k2_13,k2_21,k2_22,k2_23,k2_31,k2_32,k2_33,
k3_11,k3_12,k3_13,k3_21,k3_22,k3_23,k3_31,k3_32,k3_33,
k4_11,k4_12,k4_13,k4_21,k4_22,k4_23,k4_31,k4_32,k4_33;

u1=w_x*pi/180 ,u2=w_y*pi/180 ,u3=w_z*pi/180;

F_a11=a21*u3-a31*u2; F_a12=a22*u3-a32*u2; F_a13=a23*u3-a33*u2;
F_a21=a31*u1-a11*u3; F_a22=a32*u1-a12*u3; F_a23=a33*u1-a13*u3;
F_a31=a11*u2-a21*u1; F_a32=a12*u2-a22*u1; F_a33=a13*u2-a23*u1;

k1_11=time*F_a11; k1_12=time*F_a12; k1_13=time*F_a13; k1_21=time*F_a21; k1_22=time*F_a22; k1_23=time*F_a23;
k1_31=time*F_a31; k1_32=time*F_a32; k1_33=time*F_a33;

a11=a11+k1_11/2; a12=a12+k1_12/2; a13=a13+k1_13/2; a21=a21+k1_21/2; a22=a22+k1_22/2; a23=a23+k1_23/2;
a31=a31+k1_31/2; a32=a32+k1_32/2; a33=a33+k1_33/2;

F_a11=a21*u3-a31*u2; F_a12=a22*u3-a32*u2; F_a13=a23*u3-a33*u2;
F_a21=a31*u1-a11*u3; F_a22=a32*u1-a12*u3; F_a23=a33*u1-a13*u3;
F_a31=a11*u2-a21*u1; F_a32=a12*u2-a22*u1; F_a33=a13*u2-a23*u1;

k2_11=time*F_a11; k2_12=time*F_a12; k2_13=time*F_a13; k2_21=time*F_a21; k2_22=time*F_a22; k2_23=time*F_a23;
k2_31=time*F_a31; k2_32=time*F_a32; k2_33=time*F_a33;

a11=a11+k2_11/2; a12=a12+k2_12/2; a13=a13+k2_13/2; a21=a21+k2_21/2; a22=a22+k2_22/2; a23=a23+k2_23/2;
a31=a31+k2_31/2; a32=a32+k2_32/2; a33=a33+k2_33/2;

F_a11=a21*u3-a31*u2; F_a12=a22*u3-a32*u2; F_a13=a23*u3-a33*u2;
F_a21=a31*u1-a11*u3; F_a22=a32*u1-a12*u3; F_a23=a33*u1-a13*u3;
F_a31=a11*u2-a21*u1; F_a32=a12*u2-a22*u1; F_a33=a13*u2-a23*u1;

k3_11=time*F_a11; k3_12=time*F_a12; k3_13=time*F_a13; k3_21=time*F_a21; k3_22=time*F_a22; k3_23=time*F_a23;
k3_31=time*F_a31; k3_32=time*F_a32; k3_33=time*F_a33;

a11=a11+k3_11; a12=a12+k3_12; a13=a13+k3_13; a21=a21+k3_21; a22=a22+k3_22; a23=a23+k3_23;
a31=a31+k3_31; a32=a32+k3_32; a33=a33+k3_33;

F_a11=a21*u3-a31*u2; F_a12=a22*u3-a32*u2; F_a13=a23*u3-a33*u2;
F_a21=a31*u1-a11*u3; F_a22=a32*u1-a12*u3; F_a23=a33*u1-a13*u3;
F_a31=a11*u2-a21*u1; F_a32=a12*u2-a22*u1; F_a33=a13*u2-a23*u1;

k4_11=time*F_a11; k4_12=time*F_a12; k4_13=time*F_a13; k4_21=time*F_a21; k4_22=time*F_a22; k4_23=time*F_a23;
k4_31=time*F_a31; k4_32=time*F_a32; k4_33=time*F_a33;

b11=b11+(k1_11+2*k2_11+2*k3_11+k4_11)/6;
b12=b12+(k1_12+2*k2_12+2*k3_12+k4_12)/6;
b13=b13+(k1_13+2*k2_13+2*k3_13+k4_13)/6;
b21=b21+(k1_21+2*k2_21+2*k3_21+k4_21)/6;
b22=b22+(k1_22+2*k2_22+2*k3_22+k4_22)/6;
b23=b23+(k1_23+2*k2_23+2*k3_23+k4_23)/6;
b31=b31+(k1_31+2*k2_31+2*k3_31+k4_31)/6;
b32=b32+(k1_32+2*k2_32+2*k3_32+k4_32)/6;
b33=b33+(k1_33+2*k2_33+2*k3_33+k4_33)/6;

phi=atan(-b32/b22);
cj = cos(phi);
theta=atan2(b12*cj,b22);
psi=atan2(-b13,b11);

ps=psi*(180/pi);
ph=phi*(180/pi);
th=theta*(180/pi);

ps_d=(ps-ps_old)/time;
ph_d=(ph-ph_old)/time;
th_d=(th-th_old)/time;

ps_old=ps;
ph_old=ph;
th_old=th;
}