search

tomojitakasu / RTKLIB

2.58k stars 1.63k forks source link

Fixed matrix multiply algorithm. #639

Closed iucgnaw closed 2 years ago

iucgnaw commented 2 years ago

Derive the [3x3][33][31] matrix multiply according to BeiDou ICD, and seems like the final algorithm should be as fixed.

tomojitakasu commented 2 years ago

What ICD do you refer to ?

iucgnaw commented 2 years ago

I am referring the latest B1I ICD: http://en.beidou.gov.cn/SYSTEMS/ICD/201902/P020190227702348791891.pdf Table 5-11 Ephemeris algorithm for user

tomojitakasu commented 2 years ago

Thanks. I checked the current codes but didn't find any problem.

iucgnaw commented 2 years ago

Maybe I am wrong... I detailed my algorithm as below, then eliminated 1.0, 0.0 relevant items, and it comes out with my result. Could you kindly check what is the mistake in below matrix computing code? Thanks in advance.

double OMEGA_DOT__e_multiply_t__k = OMEGA_DOT__e * t__k;
double R__X_Sin_phi = sin(OMEGA_DOT__e_multiply_t__k);
double R__X_Cos_phi = cos(OMEGA_DOT__e_multiply_t__k);
double R__X_0_0 = 1.0;
double R__X_0_1 = 0.0;
double R__X_0_2 = 0.0;
double R__X_1_0 = 0.0;
double R__X_1_1 = +R__X_Cos_phi;
double R__X_1_2 = +R__X_Sin_phi;
double R__X_2_0 = 0.0;
double R__X_2_1 = -R__X_Sin_phi;
double R__X_2_2 = +R__X_Cos_phi;

static const double Sin_minus_5_Degree = -0.08715574274765836;  // sin(-5.0 / Degrees_per_Radian)
static const double Cos_minus_5_Degree = 0.99619469809174555;   // cos(-5.0 / Degrees_per_Radian)
double R__Z_Sin_phi = Sin_minus_5_Degree;
double R__Z_Cos_phi = Cos_minus_5_Degree;
double R__Z_0_0 = +R__Z_Cos_phi;
double R__Z_0_1 = +R__Z_Sin_phi;
double R__Z_0_2 = 0.0;
double R__Z_1_0 = -R__Z_Sin_phi;
double R__Z_1_1 = +R__Z_Cos_phi;
double R__Z_1_2 = 0.0;
double R__Z_2_0 = 0.0;
double R__Z_2_1 = 0.0;
double R__Z_2_2 = 1.0;

double R__X_x_R__Z__0_0 = R__X_0_0 * R__Z_0_0 + R__X_0_1 * R__Z_1_0 + R__X_0_2 * R__Z_2_0;
double R__X_x_R__Z__0_1 = R__X_0_0 * R__Z_0_1 + R__X_0_1 * R__Z_1_1 + R__X_0_2 * R__Z_2_1;
double R__X_x_R__Z__0_2 = R__X_0_0 * R__Z_0_2 + R__X_0_1 * R__Z_1_2 + R__X_0_2 * R__Z_2_2;
double R__X_x_R__Z__1_0 = R__X_1_0 * R__Z_0_0 + R__X_1_1 * R__Z_1_0 + R__X_1_2 * R__Z_2_0;
double R__X_x_R__Z__1_1 = R__X_1_0 * R__Z_0_1 + R__X_1_1 * R__Z_1_1 + R__X_1_2 * R__Z_2_1;
double R__X_x_R__Z__1_2 = R__X_1_0 * R__Z_0_2 + R__X_1_1 * R__Z_1_2 + R__X_1_2 * R__Z_2_2;
double R__X_x_R__Z__2_0 = R__X_2_0 * R__Z_0_0 + R__X_2_1 * R__Z_1_0 + R__X_2_2 * R__Z_2_0;
double R__X_x_R__Z__2_1 = R__X_2_0 * R__Z_0_1 + R__X_2_1 * R__Z_1_1 + R__X_2_2 * R__Z_2_1;
double R__X_x_R__Z__2_2 = R__X_2_0 * R__Z_0_2 + R__X_2_1 * R__Z_1_2 + R__X_2_2 * R__Z_2_2;

x__k = R__X_x_R__Z__0_0 * X__GK + R__X_x_R__Z__0_1 * Y__GK + R__X_x_R__Z__0_2 * Z__GK;
y__k = R__X_x_R__Z__1_0 * X__GK + R__X_x_R__Z__1_1 * Y__GK + R__X_x_R__Z__1_2 * Z__GK;
z__k = R__X_x_R__Z__2_0 * X__GK + R__X_x_R__Z__2_1 * Y__GK + R__X_x_R__Z__2_2 * Z__GK;
tomojitakasu commented 2 years ago

The multiplication order of Rz and Rx may be flipped.

iucgnaw commented 2 years ago

Yes you are right, I get same result after fix it. Thank you!