public static Position RotatePositionFromInertialFrameToBodyFrame(Orientation orientation, Position currentPosition) { Position newPosition = new Position(); double[,] rotationMatrix = GetRotationMatrixFromInertialToBodyFrame(orientation); double[,] positionMatrix = currentPosition.GetPositionMatrix(); double[,] rotatedPosition = new double[,] { { 0 }, { 0 }, { 0 } }; /*rotatedPosition[0,0] = rotationMatrix[0,0]*positionMatrix[0,0] + rotationMatrix[0,1]*positionMatrix[1,0] + rotationMatrix[0,2]*positionMatrix[2,0]; rotatedPosition[1,0] = rotationMatrix[1,0]*positionMatrix[0,0] + rotationMatrix[1,1]*positionMatrix[1,0] + rotationMatrix[1,2]*positionMatrix[2,0]; rotatedPosition[2,0] = rotationMatrix[2,0]*positionMatrix[0,0] + rotationMatrix[2,1]*positionMatrix[1,0] + rotationMatrix[2,2]*positionMatrix[2,0]; */ //for each column in rotation matrix through columns for (int i = 0; i < rotationMatrix.GetLength(1); i++) { for (int j = 0; j < positionMatrix.GetLength(0); j++) { rotatedPosition[i, 0] += rotationMatrix[i, j] * positionMatrix[j, 0]; } } newPosition.XLatitudePosition = rotatedPosition[0, 0]; newPosition.YLongitudePosition = rotatedPosition[1, 0]; newPosition.ZAltitudePosition = rotatedPosition[2, 0]; return newPosition; }
//public static double[] ConvertFromECEFToLocalNED(double[] ecefReferencePosition, double[] ecefCurrentPosition) //{ // double[,] rotationMatrix = GetRotationMatrixFromInertialToBodyFrame(orientation); // double[,] positionMatrix = currentPosition.GetPositionMatrix(); // double[,] rotatedPosition = new double[,] { { 0 }, { 0 }, { 0 } }; // /*rotatedPosition[0,0] = rotationMatrix[0,0]*positionMatrix[0,0] + rotationMatrix[0,1]*positionMatrix[1,0] + rotationMatrix[0,2]*positionMatrix[2,0]; // rotatedPosition[1,0] = rotationMatrix[1,0]*positionMatrix[0,0] + rotationMatrix[1,1]*positionMatrix[1,0] + rotationMatrix[1,2]*positionMatrix[2,0]; // rotatedPosition[2,0] = rotationMatrix[2,0]*positionMatrix[0,0] + rotationMatrix[2,1]*positionMatrix[1,0] + rotationMatrix[2,2]*positionMatrix[2,0]; // */ // //for each column in rotation matrix through columns // for (int i = 0; i < rotationMatrix.GetLength(1); i++) // { // for (int j = 0; j < positionMatrix.GetLength(0); j++) // { // rotatedPosition[i, 0] += rotationMatrix[i, j] * positionMatrix[j, 0]; // } // } // newPosition.XLatitudePosition = rotatedPosition[0, 0]; // newPosition.YLongitudePosition = rotatedPosition[1, 0]; // newPosition.ZAltitudePosition = rotatedPosition[2, 0]; // return newPosition; //} public static Position RotatePositionFromGeodedicFrameToBodyFrame(Orientation orientation, Position currentPosition, Position originalReferencePosition) { //Rotate from Geodedic to earth centered earth fixed /* Position currentEcefPosition = new Position(); double[] ecef = ConvertFromGeodeticToECEF(currentPosition.YLongitudePosition, currentPosition.XLatitudePosition, currentPosition.ZAltitudePosition); currentEcefPosition.XLatitudePosition = ecef[0]; currentEcefPosition.YLongitudePosition = ecef[1]; currentEcefPosition.ZAltitudePosition = ecef[2];*/ /* Position referenceEcefPosition = new Position(); ecef = ConvertFromGeodeticToECEF(originalReferencePosition.YLongitudePosition, originalReferencePosition.XLatitudePosition, originalReferencePosition.ZAltitudePosition); referenceEcefPosition.XLatitudePosition = ecef[0]; referenceEcefPosition.YLongitudePosition = ecef[1]; referenceEcefPosition.ZAltitudePosition = ecef[2]; */ //Rotate from earth centered earth fixed to local NED (which is the same as vehicle carried NED) double[,] ecefToNEDRotationMatrix = calculateEcefToNEDMatrix(originalReferencePosition); Position currentMinusRef = new Position(); currentMinusRef.XLatitudePosition = currentPosition.XLatitudePosition - originalReferencePosition.XLatitudePosition; currentMinusRef.YLongitudePosition = currentPosition.YLongitudePosition - originalReferencePosition.YLongitudePosition; currentMinusRef.ZAltitudePosition = currentPosition.ZAltitudePosition - originalReferencePosition.ZAltitudePosition; double[,] positionMatrix = currentMinusRef.GetPositionMatrix(); double[,] rotatedPosition = new double[,] { { 0 }, { 0 }, { 0 } }; Position currentPositionLocalNED = new Position(); //for each column in rotation matrix through columns for (int i = 0; i < ecefToNEDRotationMatrix.GetLength(1); i++) { for (int j = 0; j < positionMatrix.GetLength(0); j++) { rotatedPosition[i, 0] += ecefToNEDRotationMatrix[i, j] * positionMatrix[j, 0]; } } currentPositionLocalNED.XLatitudePosition = rotatedPosition[0, 0]; currentPositionLocalNED.YLongitudePosition = rotatedPosition[1, 0]; currentPositionLocalNED.ZAltitudePosition = rotatedPosition[2, 0]; //Rotate from vehicle carried NED to body NED double[,] NEDToBodyRotationMatrix = calculateNEDToBodyMatrix(orientation); positionMatrix = currentPositionLocalNED.GetPositionMatrix(); rotatedPosition = new double[,] { { 0 }, { 0 }, { 0 } }; Position currentPositionBody = new Position(); //for each column in rotation matrix through columns for (int i = 0; i < NEDToBodyRotationMatrix.GetLength(1); i++) { for (int j = 0; j < positionMatrix.GetLength(0); j++) { rotatedPosition[i, 0] += NEDToBodyRotationMatrix[i, j] * positionMatrix[j, 0]; } } currentPositionBody.XLatitudePosition = rotatedPosition[0, 0]; currentPositionBody.YLongitudePosition = rotatedPosition[1, 0]; currentPositionBody.ZAltitudePosition = rotatedPosition[2, 0]; return currentPositionBody; }