PosCalibrate/calPoint.h

#ifndef CALPOINT_H
#define CALPOINT_H
#include "qglobal.h"
#include <cmath>
#include "QDebug"
// 定义常量
const double Re = 6378.14;  // 地球半径，km
const double PI = 3.14159265358979323846;
const double Heading_IMUBase =63.888;
const double Pitch_IMUBase = -0.121;
const double Roll_IMUBase = -0.082;

const double Heading_OPABase=64.062;
const double Pitch_OPABase = -0.332;
const double Roll_OPABase = 89.812;

const double OpticalAxisAngle_N0=90;
const double OpticalAxisAngle_E0= 90;
const double OpticalAxisAngle_D0=0;

// 角度转弧度函数
double deg2rad(double deg) {
    return deg * PI / 180.0;
}

// 弧度转角度函数
double rad2deg(double rad) {
    return rad * 180.0 / PI;
}

// 模运算函数
double mod(double x, double y) {
    return x - y * std::floor(x / y);
}

// 矩阵乘法函数
void matrixMultiply(double A[3][3], double B[3][3], double result[3][3]) {
    for(int i=0; i<3; i++) {
        for(int j=0; j<3; j++) {
            result[i][j] = 0;
            for(int k=0; k<3; k++) {
                result[i][j] += A[i][k] * B[k][j];
            }
        }
    }
}

void matrixMultiplyVector(double A[3][3], double B[3], double result[3]) {
    for(int i=0; i<3; i++) {
        result[i] = 0;
        for(int j=0; j<3; j++) {
            result[i] += A[i][j] * B[j];
        }
    }
}

QVector<double> calRes(double Lon_A,double Lat_A,double Hei_A,double Lon_B,double Lat_B, double Hei_B,double HeadingReal,double PitchReal,double RollReal){
    // 将角度由度转化为弧度
    double Lon_A_Rad = deg2rad(Lon_A);  // A机经度，rad
    double Lat_A_Rad = deg2rad(Lat_A);  // A机纬度，rad
    double Lon_B_Rad = deg2rad(Lon_B);  // B机经度，rad
    double Lat_B_Rad = deg2rad(Lat_B);  // B机纬度，rad

    // 双机位置（地心直角坐标系）及双机距离，km
    double X_A = (Re + Hei_A) * std::cos(Lat_A_Rad) * std::cos(Lon_A_Rad);
    double Y_A = (Re + Hei_A) * std::cos(Lat_A_Rad) * std::sin(Lon_A_Rad);
    double Z_A = (Re + Hei_A) * std::sin(Lat_A_Rad);

    double X_B = (Re + Hei_B) * std::cos(Lat_B_Rad) * std::cos(Lon_B_Rad);
    double Y_B = (Re + Hei_B) * std::cos(Lat_B_Rad) * std::sin(Lon_B_Rad);
    double Z_B = (Re + Hei_B) * std::sin(Lat_B_Rad);

    // B机相对于A机的直角坐标，km
    double XYZ_BA[3] = {X_B - X_A, Y_B - Y_A, Z_B - Z_A};

    // 双机距离，km
    double D_BA = std::sqrt(XYZ_BA[0]*XYZ_BA[0] + XYZ_BA[1]*XYZ_BA[1] + XYZ_BA[2]*XYZ_BA[2]);
    qDebug()<< "双机距离: "<<D_BA<<"km";

    // 根据三轴坐标旋转矩阵计算B机相对于A机的方位角、俯仰角
    double Lon_A_Rad1 = Lon_A_Rad;
    double Lat_A_Rad1 = -Lat_A_Rad;

    double MR_X[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};

    double MR_Y[3][3] = {
        {std::cos(Lat_A_Rad1), 0, -std::sin(Lat_A_Rad1)},
        {0, 1, 0},
        {std::sin(Lat_A_Rad1), 0, std::cos(Lat_A_Rad1)}
    };

    double MR_Z[3][3] = {
        {std::cos(Lon_A_Rad1), std::sin(Lon_A_Rad1), 0},
        {-std::sin(Lon_A_Rad1), std::cos(Lon_A_Rad1), 0},
        {0, 0, 1}
    };

    // 矩阵乘法：MR_X * MR_Y * MR_Z
    double temp1[3][3], MR_XYZ[3][3];
    matrixMultiply(MR_X, MR_Y, temp1);
    matrixMultiply(temp1, MR_Z, MR_XYZ);

    // 矩阵乘法：MR_XYZ * XYZ_BA  天东北坐标
    double UEN_BA0[3];
    matrixMultiplyVector(MR_XYZ, XYZ_BA, UEN_BA0);

    double NED_BA0[3];
    NED_BA0[0] = UEN_BA0[2];
    NED_BA0[1] = UEN_BA0[1];
    NED_BA0[2]= -UEN_BA0[0];

    // POS姿态角度换算为弧度
    double HeadingReal_Rad = deg2rad(Heading_OPABase-Heading_IMUBase-HeadingReal);
    double PitchReal_Rad = deg2rad(Pitch_OPABase-Pitch_IMUBase-PitchReal);
    double RollReal_Rad = deg2rad(Roll_OPABase-Roll_IMUBase-RollReal);

    // 由实时姿态计算位置的坐标旋转矩阵
    double NED_MR_D[3][3] = {
        {std::cos(HeadingReal_Rad), -std::sin(HeadingReal_Rad), 0},
        {std::sin(HeadingReal_Rad), std::cos(HeadingReal_Rad), 0},
        {0, 0, 1}
    };

    double NED_MR_E[3][3] = {
        {std::cos(PitchReal_Rad), 0, std::sin(PitchReal_Rad)},
        {0, 1, 0},
        {-std::sin(PitchReal_Rad), 0, std::cos(PitchReal_Rad)}
    };

    double NED_MR_N[3][3] = {
        {1, 0, 0},
        {0, std::cos(RollReal_Rad), -std::sin(RollReal_Rad)},
        {0, std::sin(RollReal_Rad), std::cos(RollReal_Rad)}
    };



    double temp2[3][3], NED_XYZ[3][3];
    matrixMultiply(NED_MR_N, NED_MR_E, temp2);
    matrixMultiply(temp2, NED_MR_D, NED_XYZ);



    // 矩阵乘法：MR_XYZ * XYZ_BA  天东北坐标
    double NED_BA_Real[3];
    matrixMultiplyVector(NED_XYZ, NED_BA0, NED_BA_Real);

    double NED_N_BA_Real=NED_BA_Real[0];
    double NED_E_BA_Real=NED_BA_Real[1];
    double NED_D_BA_Real=NED_BA_Real[2];

    double BARotate_N = rad2deg(atan2(NED_D_BA_Real,NED_E_BA_Real))-OpticalAxisAngle_N0;
    double BARotate_E = rad2deg(atan2(NED_D_BA_Real,NED_N_BA_Real))-OpticalAxisAngle_E0;

    double OpticalAxisAngle_D0_Rad = deg2rad(OpticalAxisAngle_D0);
    double OpticalAxisRotate_N = std::cos(OpticalAxisAngle_D0_Rad)*BARotate_N-std::sin(OpticalAxisAngle_D0_Rad)*BARotate_E;
    double OpticalAxisRotate_E = std::sin(OpticalAxisAngle_D0_Rad)*BARotate_N+std::cos(OpticalAxisAngle_D0_Rad)*BARotate_E;

            qDebug()<<"OpticalAxisRotate_N is "<< OpticalAxisRotate_N;
    qDebug()<<"OpticalAxisRotate_E is "<< OpticalAxisRotate_E;
    // 输出结果
    QVector<double> res;
    res.append(OpticalAxisRotate_E);
    res.append(OpticalAxisRotate_N);
    return res;
}

#endif // CALPOINT_H