double[] v; //три проэкции вектора абсолютной скорости ПО на оси СИСК {Х0} (м/с)
public FINS(ConsumerParameters inputPars)
{
input = new ConsumerParameters(inputPars);
T = 0;
Omega = new double[3];
q = MathExpansion.a / (Math.Sqrt(1 - MathExpansion.epsilon * MathExpansion.epsilon
* Math.Sin(input.Phi) * Math.Sin(input.Phi)));
v_z = MathExpansion.om * q * Math.Cos(input.Phi);
r = (q + input.H) * Math.Cos(input.Phi);
n0 = Quaternion.Proud(new Quaternion(Math.Cos(input.Lambda / 2), Math.Sin(input.Lambda / 2), 0, 0),
new Quaternion(Math.Cos(input.Phi / 2), 0, 0, (-1) * Math.Sin(input.Phi / 2)));
v = new double[3];
v[0] = input.V_n;
v[1] = input.V_h;
v[2] = input.V_e + v_z;
V = Quaternion.InverseBasisTransform(n0, v);
R_vec = new double[3];
R_vec[0] = ((1 - MathExpansion.epsilon * MathExpansion.epsilon) * q + input.H) * Math.Sin(input.Phi);
R_vec[1] = r * Math.Cos(input.Lambda);
R_vec[2] = r * Math.Sin(input.Lambda);
L = Quaternion.Proud(new Quaternion(Math.Cos(input.Psi / 2), 0, -Math.Sin(input.Psi / 2), 0),
new Quaternion(Math.Cos(input.Theta / 2), 0, 0, Math.Sin(input.Theta / 2)));
L = Quaternion.Proud(L, new Quaternion(Math.Cos(input.Gama / 2), Math.Sin(input.Gama / 2), 0, 0));
}
public ConsumerParameters(ConsumerParameters cp)
{
Phi = cp.Phi;
H = cp.H;
Lambda = cp.Lambda;
V_n = cp.V_n;
V_h = cp.V_h;
V_e = cp.V_e;
Psi = cp.Psi;
Theta = cp.Theta;
Gama = cp.Gama;
}
public void Go(double[] nTheta, double[] nV, double nT, double tEx, List <ConsumerParameters> consParms)
{
double[] g = GravitationalAcceleratioVector(R_vec);
double[] vScecified = MathExpansion.Sum(nTheta,
MathExpansion.Multiplication(1 / 2, MathExpansion.VectorProud(nTheta, nV)));
double[] v1 = new double[3]; //3-х мерный вектор абсолютной скорости с предыдущего такта
for (int i = 0; i < v1.Length; i++)
{
v1[i] = V[i];
}
double[] nTheta1 = MathExpansion.Multiplication(nT, Omega);
Omega = MathExpansion.Division(nTheta, nT);
Quaternion s = Quaternion.Proud(n0, L);
double[] deltaV = MathExpansion.Sum(Quaternion.InverseBasisTransform(s, vScecified),
MathExpansion.Multiplication(nT, g));
V = MathExpansion.Sum(V, deltaV);
R_vec = MathExpansion.Sum(R_vec, MathExpansion.Multiplication(nT,
MathExpansion.Multiplication(1 / 2, MathExpansion.Sum(v1, V))));
double dl_0 = 1 - MathExpansion.Module(nTheta) * MathExpansion.Module(nTheta) / 8;
double[] dl = MathExpansion.Multiplication(1 / 2, nTheta);
dl = MathExpansion.Sum(dl, MathExpansion.Multiplication(-MathExpansion.Module(nTheta)
* MathExpansion.Module(nTheta) / 48, nTheta));
dl = MathExpansion.Sum(dl, MathExpansion.Multiplication(-1 / 24, MathExpansion.VectorProud(nTheta, nTheta1)));
Quaternion deltaL = new Quaternion(dl_0, dl);
deltaL.Multiplication(1 / Math.Sqrt(dl_0 * dl_0 + MathExpansion.ScalarProud(dl, dl)));
L = Quaternion.Proud(L, deltaL);
T += nT;
ConsumerParameters result = new ConsumerParameters();
ConsumerParametersCalculation cpCalc = new ConsumerParametersCalculation();
result = cpCalc.Calculate(T, R_vec, V, n0, L);
consParms.Add(result);
if (T < tEx)
{
Go(nTheta, nV, nT, tEx, consParms);
}
}
static void Main()
{
List <ConsumerParameters> consumerParameters = new List <ConsumerParameters>();
double phi0, h0, lambda0, vn0, vh0, ve0, psi0, theta0, gama0;
#region Ввод начальных данных
do
{
Console.WriteLine("Initial value of latitude:");
} while (!Double.TryParse(Console.ReadLine(), out phi0));
do
{
Console.WriteLine("Initial value of longitude:");
} while (!Double.TryParse(Console.ReadLine(), out lambda0));
do
{
Console.WriteLine("Initial value of height:");
} while (!Double.TryParse(Console.ReadLine(), out h0));
do
{
Console.WriteLine("Initial value of north velocity component:");
} while (!Double.TryParse(Console.ReadLine(), out vn0));
do
{
Console.WriteLine("Initial value of vertical velocity component:");
} while (!Double.TryParse(Console.ReadLine(), out vh0));
do
{
Console.WriteLine("Initial value of east velocity component:");
} while (!Double.TryParse(Console.ReadLine(), out ve0));
do
{
Console.WriteLine("Initial value of course angle:");
} while (!Double.TryParse(Console.ReadLine(), out psi0));
do
{
Console.WriteLine("Initial value of pitch angle:");
} while (!Double.TryParse(Console.ReadLine(), out theta0));
do
{
Console.WriteLine("Initial value of roll angle:");
} while (!Double.TryParse(Console.ReadLine(), out gama0));
#endregion
ConsumerParameters initial = new ConsumerParameters(phi0, h0, lambda0, vn0, vh0, ve0, psi0, theta0, gama0);
consumerParameters.Add(initial);
FINS fins = new FINS(initial);
double[] nTheta = new double[3];
double[] nV = new double[3];
double dt, tEx;
#region Ввод начальных данных для циклической части алгоритма
loopTheta:
Console.WriteLine("Increment of the angle of apparent rotation (like \"x y z\"):");
string s = Console.ReadLine();
string[] prms = s.Split(' ');
if (nTheta.Length != prms.Length)
{
Console.WriteLine("Incorrect data! Try again.");
goto loopTheta;
}
for (int i = 0; i < nTheta.Length; i++)
{
if (!Double.TryParse(prms[i], out nTheta[i]))
{
Console.WriteLine("Incorrect data! Try again.");
goto loopTheta;
}
}
loopV:
Console.WriteLine("Increment of apparent velocity vector (like \"x y z\"):");
s = Console.ReadLine();
prms = s.Split(' ');
if (nV.Length != prms.Length)
{
Console.WriteLine("Incorrect data! Try again.");
goto loopV;
}
for (int i = 0; i < nV.Length; i++)
{
if (!Double.TryParse(prms[i], out nV[i]))
{
Console.WriteLine("Incorrect data! Try again.");
goto loopV;
}
}
do
{
Console.WriteLine("Time between updates:");
} while (!Double.TryParse(Console.ReadLine(), out dt));
do
{
Console.WriteLine("Exit time:");
} while (!Double.TryParse(Console.ReadLine(), out tEx));
#endregion
fins.Go(nTheta, nV, dt, tEx, consumerParameters);
#region Вывод полученных данных в файл
using (StreamWriter sw = new StreamWriter("D:\\result.txt"))
{
sw.WriteLine("Phi:");
foreach (ConsumerParameters cp in consumerParameters)
{
sw.WriteLine(cp.Phi);
}
sw.WriteLine();
sw.WriteLine("Lambda:");
foreach (ConsumerParameters cp in consumerParameters)
{
sw.WriteLine(cp.Lambda);
}
sw.WriteLine();
sw.WriteLine("V_n:");
foreach (ConsumerParameters cp in consumerParameters)
{
sw.WriteLine(cp.V_n);
}
sw.WriteLine();
sw.WriteLine("V_h:");
foreach (ConsumerParameters cp in consumerParameters)
{
sw.WriteLine(cp.V_h);
}
sw.WriteLine();
sw.WriteLine("V_e:");
foreach (ConsumerParameters cp in consumerParameters)
{
sw.WriteLine(cp.V_e);
}
sw.WriteLine();
sw.WriteLine("Psi:");
foreach (ConsumerParameters cp in consumerParameters)
{
sw.WriteLine(cp.Psi);
}
sw.WriteLine();
sw.WriteLine("Theta:");
foreach (ConsumerParameters cp in consumerParameters)
{
sw.WriteLine(cp.Theta);
}
sw.WriteLine();
sw.WriteLine("Gama:");
foreach (ConsumerParameters cp in consumerParameters)
{
sw.WriteLine(cp.Gama);
}
sw.WriteLine();
}
#endregion
Console.ReadLine();
}
public ConsumerParametersCalculation()
{
parameters = new ConsumerParameters();
}
