-nan(ind)
,而不是数值。我在程序中应用了四阶龙格库塔方法,函数在
dvdt
和
drdt
中给出。
#include <iostream>
#include <fstream>
#include <cmath>
#include <iomanip>
using namespace std;
double dvdt(double t, double v);
double drdt(double t, double r);
double RKv(double t, double v, double h);
double RKr(double t, double r, double h);
double xo = 0; //initial X position
double yo = 0; //initial Y position
double x1; //final X position
double y11; //final Y position
double ti = 0; //initial time = 0
double tf = 30; //time entered
double dt = 0.01; //time step (iteration time)
double rud = 0; //rudder angle
double m = 53330750; //mass of submarine (Kg)
double Iz = 57.5; //moment of inertia of submarine (kg.m^2)
double xg = -0.012; //x position of COG
double yg = 0; // position of COG
double u = 10; // surge velocity
double v, vo = 0; //sway velocity
double r, ro = 0; // yaw rate
double psio = 0; //initial ship heading
double psi1; //final ship heading
double rudmax; // max rudder angle
double au; //surge acceleration
double av; // sway acceleration
double ar; //angular acceleration
double X; //surge force
double Y; //sway force
double N; //turning moment
double h = 1;
//matrices
double M[2][2]; //mass matrix
double Nu0[2][2]; //inertia matrix ig
double b[2][1]; //force matrix
double A[2][1]; //acceleration matrix
double V[2][1]; //velocity matrix
//maneuvering and hydrodynamic coefficents
double Xu1 = -1.0467 * pow(10, -3);
double Yv1 = -23.889 * pow(10, -3);
double Yr1 = -1.0510 * pow(10, -3);
double Nv1 = 1.1531 * pow(10, -3);
double Nr1 = -0.50717 * pow(10, -3);
double Xu = -2.8763 * pow(10, -3);
double Yv = -38.948 * pow(10, -3);
double Yr = 2.0031 * pow(10, -3);
double Nv = -14.287 * pow(10, -3);
double Nr = -4.2267 * pow(10, -3);
double Yrud = 1.4308 * pow(10, -3);
double Nrud = -0.71540 * pow(10, -3);
double a1 = m - Yv1;
double b1 = m * xg - Yr1;
double a2 = m * xg - Yr1;
double b2 = Iz - Nr1;
double det = a1 * b2 - a2 * b1;
double q1 = (-Yv * (Iz - Nr1) / det);
double q2 = ((m - Xu1) * u - Yr) * (-m * xg - Yr1) / det;
double q3 = ((Xu1 - Yv1) * u - Nv) * (-m * xg + Yr) / det;
double q4 = ((m * xg - Yr1) * u - Nr)* (m - Yr1) / det;
int main()
{
while (ti <= tf)
{
vo = RKv(ti, vo, h);
ro = RKr(ti, ro, h);
v += vo;
r += ro;
cout << "v = " << v << " " << "r = " << r << "\n";
ti = ti + 0.5;
rud += 0.05;
}
}
double RKv(double t, double v, double h)
{
double k1, k2, k3, k4, k5;
for (int i = 0.1; i <= h; i++)
{
k1 = h * dvdt(t, v);
k2 = h * dvdt(t + 0.5 * h, v + 0.5 * k1);
k3 = h * dvdt(t + 0.5 * h, v + 0.5 * k2);
k4 = h * dvdt(t + h, v + k3);
t = t + h;
v = v + (1.0 / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4);
}
return v;
}
double RKr(double t, double r, double h)
{
double k1, k2, k3, k4, k5;
for (int i = 1; i <= h; i++)
{
k1 = h * drdt(t, r);
k2 = h * drdt(t + 0.5 * h, r + 0.5 * k1);
k3 = h * drdt(t + 0.5 * h, r + 0.5 * k2);
k4 = h * drdt(t + h, r + k3);
t = t + h;
r = r + (1.0 / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4);
}
return r;
}
double dvdt(double t, double v)
{
// return(-Yrud * rud - (-Yv * (Iz - Nr1) / det * v + ((m - Xu1) * u - Yr) * (-m * xg - Yr1) / det * r));
return(-Yrud * rud - q1 * v + q2 * r);
}
double drdt(double t, double r)
{
//return(-Nrud * rud - (((Xu1 - Yv1) * u - Nv) * (-m * xg + Yr) / det * v + ((m * xg - Yr1) * u - Nr) * (m - Yr1) / det * r));
return(-Nrud * rud - q3 * v + q4 * r);
}
以下是该计划的结果。我得到前几次迭代的数值,之后我得到 -nan(ind)
: