Hi guys

Back again with reverse transformations for my RLG INS simulation:

In the above image, an aircraft is depicted by the set of axes on the left, at 29.54233° latitude on the earth, with a heading of 150.12° and a pitch and bank of 0. The RLGs detect the earth's rotation rate of 15°/hr and the P/R/Y rate values indicate the earth rate sensed by each RLG in its respective axis.

By reversing the pitch and roll transformation, and then taking the atan2 of the resulting pitch and roll rates, I can calculate the aircraft true heading (the CH value. Ignore the difference between it and actual heading. That's due to a normally distributed error applied to the calculation to simulate gyro inaccuracies). However, by taking the sin^-1 of the yaw rate/earth rate, I can calculate the aircraft latitude. This works all well and fine when pitch is 0, but quickly goes haywire if I change pitch. I know my other transforms are correct because the calculated heading is always consistent with the actual heading. Could someone spot the error in my reverse transformation for the yaw rate in the code below?

Thanks

JB

	double cp = cos(D2A(att1[0])), sp = sin(D2A(att1[0]));//pitch
	double cb = cos(D2A(att1[1])), sb = sin(D2A(att1[1]));//bank
	double ch = cos(D2A(att1[2])), sh = sin(D2A(att1[2]));//heading

	double clat = cos(D2A(lat - 90.0)), slat = sin(D2A(lat - 90.0));

	double mat1[3][3];
	double mat2[3][3];
	mat1[0][0] = cp*cb;
	mat1[1][0] = cp*sb;
	mat1[2][0] = -sp;

	mat1[0][1] = sh*sp*cb - ch*sb;
	mat1[1][1] = sh*sp*sb + ch*cb;
	mat1[2][1] = sh*cp;

	mat1[0][2] = ch*sp*cb + sh*sb;
	mat1[1][2] = ch*sp*sb - sh*cb;
	mat1[2][2] = ch*cp;

	mat2[0][0] = clat;
	mat2[1][0] = 0;
	mat2[2][0] = -slat;

	mat2[0][1] = 0;
	mat2[1][1] = 0;
	mat2[2][1] = 0;

	mat2[0][2] = 0;
	mat2[1][2] = 0;
	mat2[2][2] = 0;

	//Transform pitch/roll/yaw matrix by latitude matrix
	double mat3[3][3] =
	{
		mat1[0][0]*mat2[0][0] + mat1[0][1]*mat2[1][0] + mat1[0][2]*mat2[2][0], mat1[0][0]*mat2[0][1] + mat1[0][1]*mat2[1][1] + mat1[0][2]*mat2[2][1], mat1[0][0]*mat2[0][2] + mat1[0][1]*mat2[1][2] + mat1[0][2]*mat2[2][2], 
		mat1[1][0]*mat2[0][0] + mat1[1][1]*mat2[1][0] + mat1[1][2]*mat2[2][0], mat1[1][0]*mat2[0][1] + mat1[1][1]*mat2[1][1] + mat1[1][2]*mat2[2][1], mat1[1][0]*mat2[0][2] + mat1[1][1]*mat2[1][2] + mat1[1][2]*mat2[2][2], 
		mat1[2][0]*mat2[0][0] + mat1[2][1]*mat2[1][0] + mat1[2][2]*mat2[2][0], mat1[2][0]*mat2[0][1] + mat1[2][1]*mat2[1][1] + mat1[2][2]*mat2[2][1], mat1[2][0]*mat2[0][2] + mat1[2][1]*mat2[1][2] + mat1[2][2]*mat2[2][2]
	};

	//Transform earth rate by pitch/roll/yaw/heading matrix
	double rate[3] =
	{
		15.0 * mat3[1][0] + 15.0 * mat3[1][1] + 15.0 * mat3[1][2],//Pitch rate
		15.0 * mat3[0][0] + 15.0 * mat3[0][1] + 15.0 * mat3[0][2],//Yaw rate
		15.0 * mat3[2][0] + 15.0 * mat3[2][1] + 15.0 * mat3[2][2],//Roll rate
	};

	double pr = rate[0] * cb - rate[1] * sb;//This one is fine
	double rr = rate[2] * cp + rate[1] * sp*cb + rate[0] * sb*sp;//This one is fine
	double yr = (rate[0] * sb + rate[1] * cb) * (1.0/cp);//This one is faulty. Any ideas?

	//Calculate latitude from yaw rate
	double _calclat = A2D(asin(yr / 15.0));

I didn't exactly follow what you are trying to do, but working with roll/pitch/yaw angles for any arithmetic of rotations (composition, inverse) is a nightmare. Use matrices or quaternions and those things will be trivial.

I am using matrices for the most part. But for some reason my inverse of the matrix is not behaving as expected.

Picture an sphere representing the earth in the image I posted. Then picture the aircraft (the 3 coloured lines on the left) as rotated according to its pitch/bank/heading around its own origin, then rotated in latitude around the earth's origin (3 coloured lines on the right). All while the entire assembly is rotating west to east at 15 deg/hr. The ring laser gyroscopes aligned with the aircraft's respective axes are detecting this rotation. I should then be able to take that sensed rotation and work backwards to get the aircraft true heading and latitude. As mentioned, the calculations for true heading work. The calculation for latitude only works if pitch is == 0. Solving the latter is what I am trying to do.

4 hours ago, Naruto-kun said:

I am using matrices for the most part. But for some reason my inverse of the matrix is not behaving as expected.

If it's only about rotation, you can simply transpose the matrix instead.

I do not know what you try to do (this topic is always confusing), but what usually helps is a tool function to convert to/from rotation matrices in a given euler order (XYZ, or ZYX... whatever your conventions require...) I have this, let me know it you need it...

I agree with Alvaro it is very frustrating to work with Euler angles, even if it's somewhat common for flight sims. Probably you want to do all your internal math with quats (or matrices), and you do not want to convert back and forth other than for showing HUD values, steering input, real world data,... things like that.

The reason Euler angles are so bad is you always need to think about three rotations in given order, instead of just one rotation about a single axis and angle. Gaining experience with 3D rotations is not easy in general, but Euler is the hardest model to think about, and also the slowest to calculate.

I understand that, but this is not your typical flight simulator.

It is an advanced sensor model of an inertial navigation system, where I have to calculate the influence of the earth's rotation on the ring laser gyros, which output rotation rates to be integrated later, and then add error signal values to those rotation rates according to manufacturer specs, so that I can get an integrated orientation output with a specified uncertainty that will affect navigation performance.

So there is a lot of transforming back and forth. This modelling of the sensor output is also required in order to simulate the alignment function which finds true heading, and also compares the sensed latitude with the pilot latitude input.

If there is a significant difference, then it will require a re entry of the position data and a re-alignment in order to rule out pilot error and/or a failure of the system. By applying an inverse of the orientation transform to the sensed outputs, I get accurate pitch and roll rate values which I can then use to determine true heading.

The same ought to work on the yaw rate sensor, but for some reason it doesn't unless pitch is 0 and as a result I cannot get accurate latitude values if there is the slightest non 0 pitch.

If you look at the second last line in my code sample (and the 2 preceding it) you will see the inverse transformation which is working fine for pitch and roll but not for yaw.

Uhh... the terminology alone gives me headache. I would convert anything to a global cartesian coordinate system (not a spherical one using lat/long/alt), use rotation vectors for angular velocities, and quats to perform resulting rotations and space convertions.

I have no experience with flight an navigations, so i do not know what you talk about and likely i can not help. (Last time i tried ended up in 10 pages  )

However, can it be that you integrate euler angles, or you add them to perform rotations? Those are the common pitfalls where people do wrong - i even saw this in physics books, so sorry i ask for it. In fact those things do not work, only in special cases, and your saying 'it works until one angle becomes non zero' is an indication for this. Otherwise i guess it's a problem of choosing the wrong Euler order, which also works until a third angle becomes nonzero. Utility function helps here because you can trial and error all 6 options somehow quickly.

Yaw, Pitch, and roll define the direction of heading and should be rolled up into a quaternion, this is the correct way for the data to be held and will avoid the gimble lock issues presented through the use of euler angles...

Based on your variables you look to be trying to calculate Yaw based on a 'heading', 'pitch' and 'roll'?

I've also expanded out your matrix3... and i dont know if your inputs are correct... but it the matrix simplifies based on mat1 and mat2, to be:

((cp*cb)*clat) + ((ch*sp*cb + sh*sb)*-slat),0,0
((cp*sb)*(clat)) + ((ch*sp*sb - sh*cb)*(-slat)),0,0
(-sp)*(clat) +  (ch*cp)*(-slat),0,0

(so you can check my expansion/simplification Mat3 calculation substituted out results in:

((cp*cb)*clat) + ((sh*sp*cb - ch*sb)*0) + ((ch*sp*cb + sh*sb)*-slat), ((cp*cb)*0) + ((sh*sp*cb - ch*sb)*0) + ((ch*sp*cb + sh*sb)*0), ((cp*cb)*0) + ((sh*sp*cb - ch*sb)*0) + ((ch*sp*cb + sh*sb)*0), 
(cp*sb)*(clat) + (sh*sp*sb + ch*cb)*0 + (ch*sp*sb - sh*cb)*(-slat), (cp*sb)*0 + (sh*sp*sb + ch*cb)*0 + (ch*sp*sb - sh*cb)*0, (cp*sb)*0 + (sh*sp*sb + ch*cb)*0 + (ch*sp*sb - sh*cb)*0,
(-sp)*(clat) + (sh*cp)*0 + (ch*cp)*(-slat), (-sp)*0 + (sh*cp)*0 + (ch*cp)*0, (-sp)*0 + (sh*cp)*0 + (ch*cp)*0

this would mean that none of your 'rates' are correct in the truest sense, and the occasions that it 'works' may be co-incidental.

I find it difficult to believe that coincidence is involved because I consistently get the true heading output correct and the aircraft latitude output is also consistently correct when pitch is 0. Here is the test app so you can see for yourself what happens (note, keep the pfx.hlsl shader file in the same location as the exe) : https://www.dropbox.com/s/54fr4gf22ri1n6q/EarthRateSim.zip?dl=0 

If you compare the actual heading (labelled heading in the text field) with the calculated heading (CH), you will notice they are always equal, and the heading error value (HER) is always 0. Calculated latitude (CLAT) will always be the same as Latitude, unless you change the aircraft pitch to a non zero value, or if pitch is non 0 and heading is == 90 or -90. Such consistency doesn't allow for the probabilities required for coincidence.

20 hours ago, Naruto-kun said:

 The calculation for latitude only works if pitch is == 0. Solving the latter is what I am trying to do. 

I'm confused. I thought latitude was just a position. Why does the orientation of the aircraft matter?   In any case if you have a vector pointing the direction you are going it's easy enough to normalize the pitch to zero with a couple of cross products, assuming you have the center of your world.

I will explain the process again:

1: The purpose here is to simulate the detected earth rotation rate that a set of 3 mutually orthogonal ring laser gyroscopes aligned with the aircraft's axes would detect, and then use these detected values to calculate the aircraft's latitude and true heading. An example: If the aircraft is at the equator, and is pointing to true north, the pitch and yaw rate gyroscopes would detect 0 rotation, while the roll rate gyroscope would detect a rotation rate of 15°/hr since the aircraft is in effect, barrel rolling around the earth's axis. If you were to change the heading to 90°, the roll rate would be 0 and the pitch gyro would detect a downward pitch rotation of 15°/hr. At 270°, it would be an upward pitch rotation of the same. Now back to the initial setup (latitude at 0 ie equator, pitch/bank/heading all 0), if you were to shift the latitude up or down, the detected roll rate would change by the cosine of the latitude, while the detected yaw rate would change by the sine of the latitude. It is this latter factor that I am attempting to resolve since this is part of the alignment error checking mechanism in modern inertial navigation systems. Eg If the pilot-entered initial latitude is correct, and the RLG detected latitude doesn't match, then there is likely something faulty with the system. Vice versa, the pilot needs to make sure his initial position entry is correct.

2: Orientation of the aircraft will also affect influence of the earth's rotation rate on the gyroscopes. You can try it in the app in the above link. Changing the aircraft pitch while at the equator with a heading and bank of 0 will have the same effect on the roll and yaw rate gyros as changing latitude ie detected roll rate will change by the cosine of the pitch, and yaw rate by the sine of the pitch.

3: To account for non zero orientations in pitch and bank, the INS computer applies an inverse transform of the aircraft orientation to the detected rates output by the RLG units. This results in the same effect as making the aircraft truly level would. Then you can use the detected pitch and roll rates to calculate aircraft true heading, and the detected yaw rate to calculate aircraft latitude.

4: The inverse transform is working well for pitch and roll as I get a consistently correct true heading output from the calculations. But the inverse transform to the yaw rate is not working all that well since the latitude output of the calculation is only correct when pitch is 0, or heading is +-90 deg and pitch is not equal to 0.