Все снова привет. Возвращаюсь с результатами.
Я рассмотрел два случая. Первый - все четыре спутника в прямой видимости. Это позволяет составить систему из 3-х линейных уравнений, которую решаю методом Гаусса. За идею огромное спасибо пользователю [glukmaker].
Второй - один из спутников не виден. Тогда Составляется система из 3-х нелинейных уравнений. Решаю методом Ньютона. Здесь пришлось повозиться, не хотел брать готовый код, самим разобраться захотелось.
Все объединил в библиотеку, исходный код которой приведен ниже.
Header file (position_calculator.h)
#pragma once
#include <math.h>
class Space {
public:
double X = 0, Y = 0, Z = 0;
Space(double (&s1)[3], double (&s2)[3], double (&s3)[3], double (&s4)[3]);
void get_pos_3(double l1, double l2, double l3, double l4, int not_avalible);
void get_pos_4(double l1, double l2, double l3, double l4);
private:
double Sat1[3], Sat2[3], Sat3[3], Sat4[3], L1 = 0, L2 = 0, L3 = 0, L4 = 0;
void f1(double(&x)[3], double(&y)[3]);
void f2(double(&x)[3], double(&y)[3]);
void f3(double(&x)[3], double(&y)[3]);
void f4(double(&x)[3], double(&y)[3]);
void df1(double(&x)[3], double(&y)[3][3]);
void df2(double(&x)[3], double(&y)[3][3]);
void df3(double(&x)[3], double(&y)[3][3]);
void df4(double(&x)[3], double(&y)[3][3]);
void gauss(double(&A_matrix)[3][3], double(&x)[3], double(&Y_matrix)[3], double eps);
void solve(double(&start_pos)[3], double eps, int max_iteration, int not_avalible);
};
CPP file (position_calculator.cpp)
#include "position_calculator.h"
#define __X 0
#define __Y 1
#define __Z 2
Space::Space(double(&s1)[3], double(&s2)[3], double(&s3)[3], double(&s4)[3]) {
this->Sat1[__X] = s1[__X];
this->Sat1[__Y] = s1[__Y];
this->Sat1[__Z] = s1[__Z];
this->Sat2[__X] = s2[__X];
this->Sat2[__Y] = s2[__Y];
this->Sat2[__Z] = s2[__Z];
this->Sat3[__X] = s3[__X];
this->Sat3[__Y] = s3[__Y];
this->Sat3[__Z] = s3[__Z];
this->Sat4[__X] = s4[__X];
this->Sat4[__Y] = s4[__Y];
this->Sat4[__Z] = s4[__Z];
}
void Space::f1(double(&x)[3], double(&y)[3])
{
y[0] = (Sat4[__X] - x[__X]) * (Sat4[__X] - x[__X]) + (Sat4[__Y] - x[__Y]) * (Sat4[__Y] - x[__Y]) + (Sat4[__Z] - x[__Z]) * (Sat4[__Z] - x[__Z]) - L4 * L4;
y[1] = (Sat2[__X] - x[__X]) * (Sat2[__X] - x[__X]) + (Sat2[__Y] - x[__Y]) * (Sat2[__Y] - x[__Y]) + (Sat2[__Z] - x[__Z]) * (Sat2[__Z] - x[__Z]) - L2 * L2;
y[2] = (Sat3[__X] - x[__X]) * (Sat3[__X] - x[__X]) + (Sat3[__Y] - x[__Y]) * (Sat3[__Y] - x[__Y]) + (Sat3[__Z] - x[__Z]) * (Sat3[__Z] - x[__Z]) - L3 * L3;
}
void Space::df1(double(&x)[3], double(&y)[3][3])
{
y[0][__X] = -2 * (Sat4[__X] - x[__X]);
y[0][__Y] = -2 * (Sat4[__Y] - x[__Y]);
y[0][__Z] = -2 * (Sat4[__Z] - x[__Z]);
y[1][__X] = -2 * (Sat2[__X] - x[__X]);
y[1][__Y] = -2 * (Sat2[__Y] - x[__Y]);
y[1][__Z] = -2 * (Sat2[__Z] - x[__Z]);
y[2][__X] = -2 * (Sat3[__X] - x[__X]);
y[2][__Y] = -2 * (Sat3[__Y] - x[__Y]);
y[2][__Z] = -2 * (Sat3[__Z] - x[__Z]);
}
void Space::f2(double(&x)[3], double(&y)[3])
{
y[0] = (Sat1[__X] - x[__X]) * (Sat1[__X] - x[__X]) + (Sat1[__Y] - x[__Y]) * (Sat1[__Y] - x[__Y]) + (Sat1[__Z] - x[__Z]) * (Sat1[__Z] - x[__Z]) - L1 * L1;
y[1] = (Sat4[__X] - x[__X]) * (Sat4[__X] - x[__X]) + (Sat4[__Y] - x[__Y]) * (Sat4[__Y] - x[__Y]) + (Sat4[__Z] - x[__Z]) * (Sat4[__Z] - x[__Z]) - L4 * L4;
y[2] = (Sat3[__X] - x[__X]) * (Sat3[__X] - x[__X]) + (Sat3[__Y] - x[__Y]) * (Sat3[__Y] - x[__Y]) + (Sat3[__Z] - x[__Z]) * (Sat3[__Z] - x[__Z]) - L3 * L3;
}
void Space::df2(double(&x)[3], double(&y)[3][3])
{
y[0][__X] = -2 * (Sat1[__X] - x[__X]);
y[0][__Y] = -2 * (Sat1[__Y] - x[__Y]);
y[0][__Z] = -2 * (Sat1[__Z] - x[__Z]);
y[1][__X] = -2 * (Sat4[__X] - x[__X]);
y[1][__Y] = -2 * (Sat4[__Y] - x[__Y]);
y[1][__Z] = -2 * (Sat4[__Z] - x[__Z]);
y[2][__X] = -2 * (Sat3[__X] - x[__X]);
y[2][__Y] = -2 * (Sat3[__Y] - x[__Y]);
y[2][__Z] = -2 * (Sat3[__Z] - x[__Z]);
}
void Space::f3(double(&x)[3], double(&y)[3])
{
y[0] = (Sat1[__X] - x[__X]) * (Sat1[__X] - x[__X]) + (Sat1[__Y] - x[__Y]) * (Sat1[__Y] - x[__Y]) + (Sat1[__Z] - x[__Z]) * (Sat1[__Z] - x[__Z]) - L1 * L1;
y[1] = (Sat2[__X] - x[__X]) * (Sat2[__X] - x[__X]) + (Sat2[__Y] - x[__Y]) * (Sat2[__Y] - x[__Y]) + (Sat2[__Z] - x[__Z]) * (Sat2[__Z] - x[__Z]) - L2 * L2;
y[2] = (Sat4[__X] - x[__X]) * (Sat4[__X] - x[__X]) + (Sat4[__Y] - x[__Y]) * (Sat4[__Y] - x[__Y]) + (Sat4[__Z] - x[__Z]) * (Sat4[__Z] - x[__Z]) - L4 * L4;
}
void Space::df3(double(&x)[3], double(&y)[3][3])
{
y[0][__X] = -2 * (Sat1[__X] - x[__X]);
y[0][__Y] = -2 * (Sat1[__Y] - x[__Y]);
y[0][__Z] = -2 * (Sat1[__Z] - x[__Z]);
y[1][__X] = -2 * (Sat2[__X] - x[__X]);
y[1][__Y] = -2 * (Sat2[__Y] - x[__Y]);
y[1][__Z] = -2 * (Sat2[__Z] - x[__Z]);
y[2][__X] = -2 * (Sat4[__X] - x[__X]);
y[2][__Y] = -2 * (Sat4[__Y] - x[__Y]);
y[2][__Z] = -2 * (Sat4[__Z] - x[__Z]);
}
void Space::f4(double(&x)[3], double(&y)[3])
{
y[0] = (Sat1[__X] - x[__X]) * (Sat1[__X] - x[__X]) + (Sat1[__Y] - x[__Y]) * (Sat1[__Y] - x[__Y]) + (Sat1[__Z] - x[__Z]) * (Sat1[__Z] - x[__Z]) - L1 * L1;
y[1] = (Sat2[__X] - x[__X]) * (Sat2[__X] - x[__X]) + (Sat2[__Y] - x[__Y]) * (Sat2[__Y] - x[__Y]) + (Sat2[__Z] - x[__Z]) * (Sat2[__Z] - x[__Z]) - L2 * L2;
y[2] = (Sat3[__X] - x[__X]) * (Sat3[__X] - x[__X]) + (Sat3[__Y] - x[__Y]) * (Sat3[__Y] - x[__Y]) + (Sat3[__Z] - x[__Z]) * (Sat3[__Z] - x[__Z]) - L3 * L3;
}
void Space::df4(double(&x)[3], double(&y)[3][3])
{
y[0][__X] = -2 * (Sat1[__X] - x[__X]);
y[0][__Y] = -2 * (Sat1[__Y] - x[__Y]);
y[0][__Z] = -2 * (Sat1[__Z] - x[__Z]);
y[1][__X] = -2 * (Sat2[__X] - x[__X]);
y[1][__Y] = -2 * (Sat2[__Y] - x[__Y]);
y[1][__Z] = -2 * (Sat2[__Z] - x[__Z]);
y[2][__X] = -2 * (Sat3[__X] - x[__X]);
y[2][__Y] = -2 * (Sat3[__Y] - x[__Y]);
y[2][__Z] = -2 * (Sat3[__Z] - x[__Z]);
}
void Space::gauss(double(&A_matrix)[3][3], double(&x)[3], double(&Y_matrix)[3], double eps) {
int k, n, index, zeros;
zeros = 0;
k = 0;
n = 3;
double max, a[3][3], y[3];
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
a[i][j] = A_matrix[i][j];
if (a[i][j] == 0) zeros++;
}
y[i] = -Y_matrix[i];
}
if (zeros >= 5) {
for (int i = 0; i < n; i++) {
zeros = 0;
for (int j = 0; j < n; j++) {
if (a[i][j] == 0) zeros++;
else index = j;
}
if (zeros == 2) {
x[index] = y[i] / a[i][index];
}
}
x[__Z] = Sat2[__Z] - sqrt(fabs(L2 * L2 - (Sat2[__X] - x[__X]) * (Sat2[__X] - x[__X]) - (Sat2[__Y] - x[__Y]) * (Sat2[__Y] - x[__Y])));
return;
}
while (k < n)
{
// Поиск строки с максимальным a[i][k]
max = fabs(a[k][k]);
index = k;
for (int i = k + 1; i < n; i++)
{
if (fabs(a[i][k]) > max)
{
max = fabs(a[i][k]);
index = i;
}
}
// Перестановка строк
for (int j = 0; j < n; j++)
{
double temp = a[k][j];
a[k][j] = a[index][j];
a[index][j] = temp;
}
double temp = y[k];
y[k] = y[index];
y[index] = temp;
// Нормализация уравнений
for (int i = k; i < n; i++)
{
double temp = a[i][k];
if (fabs(temp) < eps) continue; // для нулевого коэффициента пропустить
for (int j = 0; j < n; j++)
a[i][j] = a[i][j] / temp;
y[i] = y[i] / temp;
if (i == k) continue; // уравнение не вычитать само из себя
for (int j = 0; j < n; j++)
a[i][j] = a[i][j] - a[k][j];
y[i] = y[i] - y[k];
}
k++;
}
// обратная подстановка
for (k = n - 1; k >= 0; k--)
{
x[k] = y[k];
for (int i = 0; i < k; i++)
y[i] = y[i] - a[i][k] * x[k];
}
}
void Space::solve(double(&start_pos)[3], double eps, int max_iteration, int not_avalible) {
double W_matrix[3][3], f_res[3], dx[3], max;
int cnt = 0;
do {
cnt++;
switch (not_avalible)
{
case 1:
f1(start_pos, f_res); // get residuals for current value of 'x'
df1(start_pos, W_matrix);
break;
case 2:
f2(start_pos, f_res); // get residuals for current value of 'x'
df2(start_pos, W_matrix);
break;
case 3:
f3(start_pos, f_res); // get residuals for current value of 'x'
df3(start_pos, W_matrix);
break;
case 4:
f4(start_pos, f_res); // get residuals for current value of 'x'
df4(start_pos, W_matrix);
break;
}
gauss(W_matrix, dx, f_res, eps);
//max = (fabs(dx[0]) > fabs(dx[1]) ? fabs(dx[0]) : fabs(dx[1]));
//max = (max > fabs(dx[2]) ? max : fabs(dx[2]));
max = fabs(dx[0]) + fabs(dx[1]) + fabs(dx[2]);
start_pos[0] = start_pos[0] + dx[0];
start_pos[1] = start_pos[1] + dx[1];
start_pos[2] = start_pos[2] + dx[2];
} while (max > eps && cnt <= max_iteration);
}
void Space::get_pos_3(double l1, double l2, double l3, double l4, int not_avalible) {
this->L1 = l1;
this->L2 = l2;
this->L3 = l3;
this->L4 = l4;
double x[3] = { X, Y, Z };
int maxiter = 300;
double eps = 0.0001;
solve(x, eps, maxiter, not_avalible);
X = round(x[__X] * 100) / 100;
Y = round(x[__Y] * 100) / 100;
Z = round(x[__Z] * 100) / 100;
switch (not_avalible)
{
case 1:
Z = Sat2[__Z] - sqrt(fabs(L2 * L2 - (Sat2[__X] - X) * (Sat2[__X] - X) - (Sat2[__Y] - Y) * (Sat2[__Y] - Y)));
break;
case 2:
Z = Sat3[__Z] - sqrt(fabs(L3 * L3 - (Sat3[__X] - X) * (Sat3[__X] - X) - (Sat3[__Y] - Y) * (Sat3[__Y] - Y)));
break;
case 3:
Z = Sat4[__Z] - sqrt(fabs(L4 * L4 - (Sat4[__X] - X) * (Sat4[__X] - X) - (Sat4[__Y] - Y) * (Sat4[__Y] - Y)));
break;
case 4:
Z = Sat1[__Z] - sqrt(fabs(L1 * L1 - (Sat1[__X] - X) * (Sat1[__X] - X) - (Sat1[__Y] - Y) * (Sat1[__Y] - Y)));
break;
}
//Z = round(Z * 100) / 100;
//while(Z > Sat2[__Z]) Z = 2 * Sat2[__Z] - Z;
//X = x[__X];
//Y = x[__Y];
//Z = x[__Z];
}
void Space::get_pos_4(double l1, double l2, double l3, double l4) {
this->L1 = l1;
this->L2 = l2;
this->L3 = l3;
this->L4 = l4;
double x[3] = { X, Y, Z };
double eps = 0.00001;
double A_matrix[3][3] = { {Sat2[__X] - Sat1[__X], Sat2[__Y] - Sat1[__Y], Sat2[__Z] - Sat1[__Z]},
{Sat3[__X] - Sat1[__X], Sat3[__Y] - Sat1[__Y], Sat3[__Z] - Sat1[__Z]},
{Sat4[__X] - Sat1[__X], Sat4[__Y] - Sat1[__Y], Sat4[__Z] - Sat1[__Z]} };
double Y_matrix[3] = { (L1 * L1 - L2 * L2 + Sat2[__X] * Sat2[__X] - Sat1[__X] * Sat1[__X] + Sat2[__Y] * Sat2[__Y] - Sat1[__Y] * Sat1[__Y] + Sat2[__Z] * Sat2[__Z] - Sat1[__Z] * Sat1[__Z]) / -2,
(L1 * L1 - L3 * L3 + Sat3[__X] * Sat3[__X] - Sat1[__X] * Sat1[__X] + Sat3[__Y] * Sat3[__Y] - Sat1[__Y] * Sat1[__Y] + Sat3[__Z] * Sat3[__Z] - Sat1[__Z] * Sat1[__Z]) / -2,
(L1 * L1 - L4 * L4 + Sat4[__X] * Sat4[__X] - Sat1[__X] * Sat1[__X] + Sat4[__Y] * Sat4[__Y] - Sat1[__Y] * Sat1[__Y] + Sat4[__Z] * Sat4[__Z] - Sat1[__Z] * Sat1[__Z]) / -2 };
gauss(A_matrix, x, Y_matrix, eps);
X = round(x[__X] * 100) / 100;
Y = round(x[__Y] * 100) / 100;
Z = round(x[__Z] * 100) / 100;
}