Относительные углы между самолетом (вектором) и точкой - PullRequest
Новая
       53

Относительные углы между самолетом (вектором) и точкой

0 голосов
/ 28 мая 2019

Я пытаюсь смоделировать камеру, прикрепленную к нижней части самолета. Иногда камера настроена на фиксирование в заданном положении (широта, долгота и высота). Используя расчеты с http://www.cosinekitty.com/compass.html,, я могу получить азимут (положительный = восток, отрицательный = запад) и угол места (положительный = вверх, отрицательный = вниз) от самолета к выбранной точке.

К сожалению, для программного обеспечения моей камеры требуются отклонение от курса, угол наклона и крен (или азимут и угол места) относительно самолета, и расчеты по http://www.cosinekitty.com/compass.html являются абсолютными (истинный север), поэтому я не могу выровнять камеру по точка, когда самолет поворачивается и / или качается.

Некоторые из доступных данных о летательном аппарате, которые могут быть полезны: широта, долгота, высота, вектор скорости x, y, z, рыскание, угол наклона и крен.

Гироскопа нет, поэтому горизонт будет под углом, когда самолет наклонится.

Мой код использует библиотеки Eigen и Boost для матриц и четвертей, но я готов попробовать что угодно, если это поможет решить эту проблему.

Я работаю над этой проблемой уже пару недель и начал забывать все разные вещи, которые я пробовал. Я начал с того, что попытался разобраться в веб-сайте Мартина Джона Бейкера (https://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm) и создал свои собственные матрицы вращения. Совсем недавно я пытался преобразовать отклонение, отклонение и крен самолета в кватернион ( Эйлер к кватерниону / кватернион к эйлеру, используя Eigen ), беря обратное значение и умножая его на вычисленные отклонение камеры (азимут), угол наклона (угол места) и крен (0,0). 

Eigen::Quaterniond qOwnship, qPoint, qResult;
Eigen::Vector3d euler;

// Initialize ownship
double ownshipRoll = convertTo(ownship->getRoll(), Angle::radian);
double ownshipPitch = convertTo(ownship->getPitch(), Angle::radian);
double ownshipYaw = convertTo(ownship->getHeading(), Angle::radian);

// Initialize point
double pointRoll = 0.0;
double pointPitch = convertTo(calculatedAltitudeAngle, Angle::radian);
double pointYaw = convertTo(calculatedAzimuth, Angle::radian);

// Convert ownship to quaternion
qOwnship = Eigen::AngleAxis<double>(ownshipRoll, Eigen::Vector3d::UnitX()) * Eigen::AngleAxis<double>(ownshipPitch, Eigen::Vector3d::UnitY()) * Eigen::AngleAxis<double>(ownshipYaw, Eigen::Vector3d::UnitZ());

// Convert point to quaternion
qPoint = Eigen::AngleAxis<double>(pointRoll, Eigen::Vector3d::UnitX()) * Eigen::AngleAxis<double>(pointPitch, Eigen::Vector3d::UnitY()) * Eigen::AngleAxis<double>(pointYaw, Eigen::Vector3d::UnitZ());

// Calculate result
qResult = qPoint * qOwnship.inverse();

// Convert quaternion to yaw, pitch, and roll
euler = qResult.toRotationMatrix().eulerAngles(0, 1, 2);

printf("Result Euler roll: %f, pitch: %f, yaw: %f\n", euler[0], euler[1], euler[2]);

Я ожидаю, что выходными данными будут рыскание, угол наклона и крен (или азимут и угол места) камеры относительно самолета, чтобы она постоянно указывала на желаемую широту, долготу и высоту.

Выходные данные могут быть результатом преобразования вычисленного азимута / угла места в относительный угол рыскания, тангажа и крена или начала очистки и использования данных самолета с широтой, долготой и высотой желаемой точки.

Любая помощь будет принята с благодарностью.

...