484 lines
12 KiB
C++
484 lines
12 KiB
C++
#include <stdlib.h>
|
||
#include <math.h>
|
||
#include <time.h>
|
||
|
||
|
||
/* 随机数 */
|
||
double randn()
|
||
{
|
||
static int hasSpare = 0;
|
||
static double spare;
|
||
|
||
if (hasSpare)
|
||
{
|
||
hasSpare = 0;
|
||
return spare;
|
||
}
|
||
else
|
||
{
|
||
hasSpare = 1;
|
||
|
||
static double u, v, s;
|
||
do
|
||
{
|
||
u = (rand() / ((double)RAND_MAX)) * 2.0 - 1.0;
|
||
v = (rand() / ((double)RAND_MAX)) * 2.0 - 1.0;
|
||
s = u * u + v * v;
|
||
} while (s >= 1 || s == 0);
|
||
|
||
s = sqrt(-2.0 * log(s) / s);
|
||
spare = v * s;
|
||
return u * s;
|
||
}
|
||
|
||
//double u;
|
||
//u = (rand() / ((double)RAND_MAX)) * 2.0 - 1.0;
|
||
//return u;
|
||
}
|
||
|
||
|
||
/* 矩阵复制函数 */
|
||
int mtxCpy(double* pMtx1, double* pMtx2, int row, int col) {
|
||
int i, j;
|
||
|
||
for (i = 0; i < row; i = i + 1)
|
||
for (j = 0; j < col; j = j + 1)
|
||
*(pMtx1 + i * col + j) = *(pMtx2 + i * col + j);
|
||
|
||
return 1;
|
||
}
|
||
|
||
|
||
/* 向量求模 */
|
||
double norm(double* pMtx0, int cnt) {
|
||
int i;
|
||
static double tmp;
|
||
|
||
tmp = 0;
|
||
for (i = 0; i < cnt; i = i + 1)
|
||
tmp = tmp + (*(pMtx0 + i)) * (*(pMtx0 + i));
|
||
tmp = sqrt(tmp);
|
||
|
||
return tmp;
|
||
}
|
||
|
||
|
||
/* 求取叉乘矩阵[Bx] */
|
||
void CrossMatrixGet(double element1[3][3], double B[3]) {
|
||
|
||
element1[0][0] = 0; element1[0][1] = -B[2]; element1[0][2] = B[1];
|
||
element1[1][0] = B[2]; element1[1][1] = 0; element1[1][2] = -B[0];
|
||
element1[2][0] = -B[1]; element1[2][1] = B[0]; element1[2][2] = 0;
|
||
|
||
return;
|
||
}
|
||
|
||
|
||
/* 四元数左乘矩阵 */
|
||
void qleft(double Q1[4][4], double qinn[4]) {
|
||
Q1[0][0] = qinn[0]; Q1[0][1] = -qinn[1]; Q1[0][2] = -qinn[2]; Q1[0][3] = -qinn[3];
|
||
Q1[1][0] = qinn[1]; Q1[1][1] = qinn[0]; Q1[1][2] = -qinn[3]; Q1[1][3] = qinn[2];
|
||
Q1[2][0] = qinn[2]; Q1[2][1] = qinn[3]; Q1[2][2] = qinn[0]; Q1[2][3] = -qinn[1];
|
||
Q1[3][0] = qinn[3]; Q1[3][1] = -qinn[2]; Q1[3][2] = qinn[1]; Q1[3][3] = qinn[0];
|
||
|
||
return;
|
||
}
|
||
|
||
|
||
/* 矩阵乘法函数 */
|
||
int mtxMtp(double* pMtx0, double* pMtx1, int row1, int col1, double* pMtx2, int row2, int col2) {
|
||
int i, j, k;
|
||
|
||
if ((row1 < 2) && (col1 < 2)) {
|
||
for (i = 0; i < row2; i = i + 1)
|
||
for (j = 0; j < col2; j = j + 1)
|
||
*(pMtx0 + i * col2 + j) = (*(pMtx1))
|
||
* (*(pMtx2 + i * col2 + j));
|
||
return 1;
|
||
}
|
||
if ((row2 < 2) && (col2 < 2)) {
|
||
for (i = 0; i < row1; i = i + 1)
|
||
for (j = 0; j < col1; j = j + 1)
|
||
*(pMtx0 + i * col1 + j) = (*(pMtx1 + i * col1 + j))
|
||
* (*(pMtx2));
|
||
return 1;
|
||
}
|
||
if (fabs(col1 - row2) < 1) {
|
||
for (i = 0; i < row1; i = i + 1)
|
||
for (j = 0; j < col2; j = j + 1) {
|
||
*(pMtx0 + i * col2 + j) = 0;
|
||
for (k = 0; k < row2; k = k + 1)
|
||
*(pMtx0 + i * col2 + j) += (*(pMtx1 + i * col1 + k))
|
||
* (*(pMtx2 + k * col2 + j));
|
||
}
|
||
return 1;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
/* 欧拉角度(312)转四元数 */
|
||
void euler2qua(double q[4], double euler_angle[3]) {
|
||
|
||
/* 欧拉角度312转四元数模块 */
|
||
/* 输入量 */
|
||
/* euler_angle 312欧拉角度 */
|
||
/* 输出量 */
|
||
/* q 四元数 */
|
||
|
||
double pufai = euler_angle[0] * 3.1415926535 / 180.0;
|
||
double fai = euler_angle[1] * 3.1415926535 / 180.0;
|
||
double theta = euler_angle[2] * 3.1415926535 / 180.0;
|
||
//计算四元数的各个分量
|
||
double q0 = cos(theta / 2) * cos(fai / 2) * cos(pufai / 2) - sin(theta / 2) * sin(fai / 2) * sin(pufai / 2);
|
||
double q1 = cos(theta / 2) * sin(fai / 2) * cos(pufai / 2) - sin(theta / 2) * cos(fai / 2) * sin(pufai / 2);
|
||
double q2 = sin(theta / 2) * cos(fai / 2) * cos(pufai / 2) + cos(theta / 2) * sin(fai / 2) * sin(pufai / 2);
|
||
double q3 = sin(theta / 2) * sin(fai / 2) * cos(pufai / 2) + cos(theta / 2) * cos(fai / 2) * sin(pufai / 2);
|
||
//将四元数存入数组中
|
||
q[0] = q0;
|
||
q[1] = q1;
|
||
q[2] = q2;
|
||
q[3] = q3;
|
||
//如果第一个分量小于0，则取负
|
||
if (q[0] < 0)
|
||
{
|
||
q[0] = -q[0];
|
||
q[1] = -q[1];
|
||
q[2] = -q[2];
|
||
q[3] = -q[3];
|
||
}
|
||
}
|
||
|
||
|
||
/* 矩阵行列式 */
|
||
double mtxDet(double* pMtx0, int n) {
|
||
int r, c, m;
|
||
int lop = 0;
|
||
double result = 0;
|
||
double mid = 1;
|
||
|
||
if (n != 1) {
|
||
lop = (n == 2) ? 1 : n;
|
||
for (m = 0; m < lop; m++) {
|
||
mid = 1;
|
||
for (r = 0, c = m; r < n; r++, c++) {
|
||
mid = mid * (*(pMtx0 + r * n + c % n));
|
||
}
|
||
result += mid;
|
||
}
|
||
for (m = 0; m < lop; m++) {
|
||
mid = 1;
|
||
for (r = 0, c = n - 1 - m + n; r < n; r++, c--) {
|
||
mid = mid * (*(pMtx0 + r * n + c % n));
|
||
}
|
||
result -= mid;
|
||
}
|
||
}
|
||
else
|
||
result = *pMtx0;
|
||
return result;
|
||
}
|
||
|
||
|
||
/* 矩阵逆*/
|
||
void mtxInv(double* pMtx0, double* pMtx1, int n) {
|
||
int i, j, k, t;
|
||
double det;
|
||
double temp[9]; //只能计算3X3矩阵的逆矩阵，当定义*temp时分配地址出错
|
||
|
||
det = mtxDet((double*)pMtx1, n);
|
||
|
||
for (i = 0; i < n; i++) {
|
||
for (j = 0; j < n; j++) {
|
||
for (k = 0; k < n - 1; k++) {
|
||
for (t = 0; t < n - 1; t++) {
|
||
if (k >= i)
|
||
if (t >= j)
|
||
*(temp + k * (n - 1) + t) = *(pMtx1 + n * (k + 1)
|
||
+ t + 1);
|
||
else
|
||
*(temp + k * (n - 1) + t) = *(pMtx1 + n * (k + 1)
|
||
+ t);
|
||
else if (t >= j)
|
||
*(temp + k * (n - 1) + t) = *(pMtx1 + n * k + t + 1);
|
||
else
|
||
*(temp + k * (n - 1) + t) = *(pMtx1 + n * k + t);
|
||
}
|
||
}
|
||
if ((i + j) % 2 == 1)
|
||
*(pMtx0 + j * n + i) = -mtxDet((double*)temp, n - 1) / det;
|
||
else
|
||
*(pMtx0 + j * n + i) = mtxDet((double*)temp, n - 1) / det;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
/* 矩阵转置 */
|
||
void mtxT(double* pMtx0, double* pMtx1, int row, int col) {
|
||
int i, j;
|
||
|
||
for (i = 0; i < row; i = i + 1)
|
||
for (j = 0; j < col; j = j + 1)
|
||
*(pMtx0 + i * col + j) = *(pMtx1 + j * row + i);
|
||
|
||
return;
|
||
}
|
||
|
||
|
||
/* 矩阵相加 */
|
||
void mtxAdd(double* pMtx0, double* pMtx1, double* pMtx2, int row, int col) {
|
||
int i, j;
|
||
|
||
for (i = 0; i < row; i = i + 1)
|
||
for (j = 0; j < col; j = j + 1)
|
||
*(pMtx0 + i * col + j) = (*(pMtx1 + i * col + j))
|
||
+ (*(pMtx2 + i * col + j));
|
||
|
||
return;
|
||
}
|
||
|
||
|
||
/* 矩阵相减 */
|
||
void mtxSub(double* pMtx0, double* pMtx1, double* pMtx2, int row, int col) {
|
||
int i, j;
|
||
|
||
for (i = 0; i < row; i = i + 1)
|
||
for (j = 0; j < col; j = j + 1)
|
||
*(pMtx0 + i * col + j) = (*(pMtx1 + i * col + j))
|
||
- (*(pMtx2 + i * col + j));
|
||
|
||
return;
|
||
}
|
||
|
||
|
||
/* 四元数表示的姿态矩阵 */
|
||
void TransQ(double Q[3][3], double q[4]) {
|
||
|
||
/* 盛一鸣，2023.08.22 */
|
||
/* 姿态矩阵四元数表达模块 */
|
||
/* 输入量 */
|
||
/* q 四元数 */
|
||
/* 输出量 */
|
||
/* Q 姿态矩阵 */
|
||
|
||
double q1[1] = { q[0] };
|
||
double q2[1] = { q[1] };
|
||
double q3[1] = { q[2] };
|
||
double q4[1] = { q[3] };
|
||
|
||
Q[0][0] = q1[0] * q1[0] + q2[0] * q2[0] - q3[0] * q3[0] - q4[0] * q4[0]; Q[0][1] = 2 * (q2[0] * q3[0] + q1[0] * q4[0]); Q[0][2] = 2 * (q2[0] * q4[0] - q1[0] * q3[0]);
|
||
Q[1][0] = 2 * (q2[0] * q3[0] - q1[0] * q4[0]); Q[1][1] = q1[0] * q1[0] - q2[0] * q2[0] + q3[0] * q3[0] - q4[0] * q4[0]; Q[1][2] = 2 * (q3[0] * q4[0] + q1[0] * q2[0]);
|
||
Q[2][0] = 2 * (q2[0] * q4[0] + q1[0] * q3[0]); Q[2][1] = 2 * (q3[0] * q4[0] - q1[0] * q2[0]); Q[2][2] = q1[0] * q1[0] - q2[0] * q2[0] - q3[0] * q3[0] + q4[0] * q4[0];
|
||
|
||
}
|
||
|
||
|
||
/* 根据轨道信息获得惯性系到轨道系的姿态矩阵 */
|
||
void MtxJtoOGet(double MtxJtoO[3][3], double orbInfo[6]) {
|
||
static double XVec[3], YVec[3], ZVec[3], VVec[3];
|
||
static double tmp, tmpM[3][3];
|
||
|
||
tmp = -1.0 / norm(orbInfo, 3);
|
||
mtxMtp(ZVec, orbInfo, 3, 1, &tmp, 1, 1);
|
||
/* 获得轨道系Z在惯性系的单位矢量 */
|
||
tmp = 1.0 / norm(&orbInfo[3], 3);
|
||
mtxMtp(VVec, &orbInfo[3], 3, 1, &tmp, 1, 1);
|
||
tmpM[0][0] = 0.0;
|
||
tmpM[0][1] = -ZVec[2];
|
||
tmpM[0][2] = ZVec[1];
|
||
tmpM[1][0] = ZVec[2];
|
||
tmpM[1][1] = 0.0;
|
||
tmpM[1][2] = -ZVec[0];
|
||
tmpM[2][0] = -ZVec[1];
|
||
tmpM[2][1] = ZVec[0];
|
||
tmpM[2][2] = 0.0;
|
||
mtxMtp(YVec, (double*)tmpM, 3, 3, VVec, 3, 1);
|
||
tmp = 1.0 / norm(YVec, 3);
|
||
mtxMtp(YVec, YVec, 3, 1, &tmp, 1, 1);
|
||
/* 获得轨道系Y在惯性系的单位矢量 */
|
||
tmpM[0][0] = 0.0;
|
||
tmpM[0][1] = -YVec[2];
|
||
tmpM[0][2] = YVec[1];
|
||
tmpM[1][0] = YVec[2];
|
||
tmpM[1][1] = 0.0;
|
||
tmpM[1][2] = -YVec[0];
|
||
tmpM[2][0] = -YVec[1];
|
||
tmpM[2][1] = YVec[0];
|
||
tmpM[2][2] = 0.0;
|
||
mtxMtp(XVec, (double*)tmpM, 3, 3, ZVec, 3, 1);
|
||
/* 获得轨道系Z在惯性系的单位矢量 */
|
||
mtxCpy(MtxJtoO[0], XVec, 1, 3);
|
||
mtxCpy(MtxJtoO[1], YVec, 1, 3);
|
||
mtxCpy(MtxJtoO[2], ZVec, 1, 3);
|
||
/* 利用XYZ矢量构成姿态矩阵 */
|
||
return;
|
||
}
|
||
|
||
|
||
void A2q(double q[4], double A[3][3]) {
|
||
double c0 = 0.5 * sqrt(1 + A[0][0] + A[1][1] + A[2][2]);
|
||
double c1 = 0.5 * sqrt(1 + A[0][0] - A[1][1] - A[2][2]);
|
||
double c2 = 0.5 * sqrt(fabs(1 - A[0][0] + A[1][1] - A[2][2]));
|
||
double c3 = 0.5 * sqrt(1 - A[0][0] - A[1][1] + A[2][2]);
|
||
|
||
if (c0 == 0) {
|
||
q[0] = c0;
|
||
q[1] = c1;
|
||
q[2] = c2;
|
||
q[3] = c3;
|
||
}
|
||
else {
|
||
q[0] = c0;
|
||
if (A[1][2] - A[2][1] >= 0) {
|
||
q[1] = c1;
|
||
}
|
||
else {
|
||
q[1] = -c1;
|
||
}
|
||
if (A[2][0] - A[0][2] >= 0) {
|
||
q[2] = c2;
|
||
}
|
||
else {
|
||
q[2] = -c2;
|
||
}
|
||
if (A[0][1] - A[1][0] >= 0) {
|
||
q[3] = c3;
|
||
}
|
||
else {
|
||
q[3] = -c3;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
/* 符号函数 */
|
||
int sign(double x) {
|
||
if (x > 0)
|
||
return 1;
|
||
else if (x == 0)
|
||
return 0;
|
||
else
|
||
return -1;
|
||
}
|
||
|
||
|
||
/* 四元数转欧拉角(312)角度制 */
|
||
void qua2euler(double euler_angle[3], double q[4], double euler_angle_last[3]) {
|
||
|
||
/* 盛一鸣，2023.08.22 */
|
||
/* 四元数转欧拉角模块 */
|
||
/* 输入量 */
|
||
/* q 四元数 */
|
||
/* euler_angle_last 欧拉角 */
|
||
/* 输出量 */
|
||
/* euler_angle 欧拉角 */
|
||
|
||
double Q[3][3];
|
||
TransQ(Q, q);
|
||
double angle_roll = asin(Q[1][2]) * 180.0 / 3.14159265358979323846;
|
||
if (fabs(angle_roll) > 89.95) {
|
||
angle_roll = sign(angle_roll) * 89.95;
|
||
euler_angle[0] = euler_angle_last[0];
|
||
euler_angle[1] = angle_roll;
|
||
euler_angle[2] = euler_angle_last[2];
|
||
}
|
||
else {
|
||
double angle_yaw = atan2(-Q[1][0], Q[1][1]) * 180.0 / 3.14159265358979323846;
|
||
double angle_pitch = atan2(-Q[0][2], Q[2][2]) * 180.0 / 3.14159265358979323846;
|
||
euler_angle[0] = angle_yaw;
|
||
euler_angle[1] = angle_roll;
|
||
euler_angle[2] = angle_pitch;
|
||
}
|
||
}
|
||
|
||
|
||
/* 矢量叉乘法 */
|
||
void VecCross(double* pMtx0, double* pMtx1, double* pMtx2) {
|
||
double Mtx[3][3];
|
||
|
||
Mtx[0][0] = 0;
|
||
Mtx[0][1] = -(*(pMtx1 + 2));
|
||
Mtx[0][2] = (*(pMtx1 + 1));
|
||
Mtx[1][0] = (*(pMtx1 + 2));
|
||
Mtx[1][1] = 0;
|
||
Mtx[1][2] = -(*pMtx1);
|
||
Mtx[2][0] = -(*(pMtx1 + 1));
|
||
Mtx[2][1] = (*pMtx1);
|
||
Mtx[2][2] = 0;
|
||
|
||
mtxMtp(pMtx0, (double*)Mtx, 3, 3, pMtx2, 3, 1);
|
||
|
||
return;
|
||
}
|
||
|
||
|
||
// ECI坐标系转到LVLH坐标系
|
||
void Fun_Mtx_ECI2LVLH(double* R, double* V, double* Mtx_ECI2LVLH) {
|
||
double I[3], J[3], K[3], H_RV[3];
|
||
double r = sqrt(R[0] * R[0] + R[1] * R[1] + R[2] * R[2]);
|
||
I[0] = R[0] / r;
|
||
I[1] = R[1] / r;
|
||
I[2] = R[2] / r;
|
||
H_RV[0] = R[1] * V[2] - R[2] * V[1];
|
||
H_RV[1] = R[2] * V[0] - R[0] * V[2];
|
||
H_RV[2] = R[0] * V[1] - R[1] * V[0];
|
||
double h_rv = sqrt(H_RV[0] * H_RV[0] + H_RV[1] * H_RV[1] + H_RV[2] * H_RV[2]);
|
||
K[0] = H_RV[0] / h_rv;
|
||
K[1] = H_RV[1] / h_rv;
|
||
K[2] = H_RV[2] / h_rv;
|
||
J[0] = K[1] * I[2] - K[2] * I[1];
|
||
J[1] = K[2] * I[0] - K[0] * I[2];
|
||
J[2] = K[0] * I[1] - K[1] * I[0];
|
||
Mtx_ECI2LVLH[0] = I[0];
|
||
Mtx_ECI2LVLH[1] = I[1];
|
||
Mtx_ECI2LVLH[2] = I[2];
|
||
Mtx_ECI2LVLH[3] = J[0];
|
||
Mtx_ECI2LVLH[4] = J[1];
|
||
Mtx_ECI2LVLH[5] = J[2];
|
||
Mtx_ECI2LVLH[6] = K[0];
|
||
Mtx_ECI2LVLH[7] = K[1];
|
||
Mtx_ECI2LVLH[8] = K[2];
|
||
}
|
||
|
||
|
||
/* ECI坐标系转LVLH坐标系 */
|
||
void FunECI2LVLH(double RLVLH[3], double RVLVLH[6], double RVChaECI[6], double RVTarECI[6])
|
||
{
|
||
double RTarECI[3], VTarECI[3];
|
||
double RChaECI[3], VChaECI[3];
|
||
double MtxECI2LVLH[3][3];
|
||
double Temp;
|
||
double Temp1[3], Temp2[3], Temp3[3];
|
||
double VLVLH[3];
|
||
double one = 1;
|
||
int ii;
|
||
|
||
for (ii = 0; ii < 3; ii++)
|
||
{
|
||
RTarECI[ii] = RVTarECI[ii];
|
||
VTarECI[ii] = RVTarECI[ii + 3];
|
||
RChaECI[ii] = RVChaECI[ii];
|
||
VChaECI[ii] = RVChaECI[ii + 3];
|
||
}
|
||
Fun_Mtx_ECI2LVLH((double*)RTarECI, (double*)VTarECI, (double*)MtxECI2LVLH); /* 计算转化矩阵Q */
|
||
VecCross(Temp1, RTarECI, VTarECI); /* H = cross(R_Tar,V_Tar) */
|
||
Temp = norm(RTarECI, 3); /* r_Tar = norm(R_Tar) */
|
||
Temp = one / (Temp * Temp); /* Temp = 1/ r_Tar^2 */
|
||
mtxMtp(Temp2, Temp1, 3, 1, &Temp, 1, 1); /* Omega = H / r_Tar^2 */
|
||
mtxSub(Temp1, RChaECI, RTarECI, 3, 1); /* RelR = R_Cha - R_Tar */
|
||
VecCross(Temp3, Temp2, Temp1); /* Temp3 = cross(Omega,RelR) */
|
||
mtxAdd(Temp2, Temp3, VTarECI, 3, 1); /* Temp2 = V_Tar + cross(Omega,RelR) */
|
||
mtxSub(Temp3, VChaECI, Temp2, 3, 1); /* RelV = V_Cha - V_Tar - cross(Omega,RelR)*/
|
||
mtxMtp(RLVLH, (double*)MtxECI2LVLH, 3, 3, (double*)Temp1, 3, 1); /* RLVLH = MtxECI2LVLH * RelR */
|
||
mtxMtp(VLVLH, (double*)MtxECI2LVLH, 3, 3, (double*)Temp3, 3, 1); /* VLVLH = MtxECI2LVLH * RelV */
|
||
for (ii = 0; ii < 3; ii++)
|
||
{
|
||
RVLVLH[ii] = RLVLH[ii];
|
||
RVLVLH[ii + 3] = VLVLH[ii];
|
||
}
|
||
}
|
||
|
||
|