绝对传感器方向

Question

我正在使用MISB KLV Local Dataset中的数据和NASA Worldwind中提供的Matrix操作。 MISB数据提供了偏航，俯仰和横滚的平台方向以及偏航，俯仰和横滚平台相对于平台的传感器方位。我试图根据平台方向和传感器相对方向来计算传感器相对于北方的绝对方位（偏航，俯仰，滚动）。

我目前正在计算平台旋转矩阵和传感器相对旋转矩阵并将结果相乘。由此产生的旋转矩阵似乎不正确。根据第6.2.4节中的MISB文件，欧拉角操作顺序是偏航，俯仰，然后滚动。结合旋转矩阵以获得绝对旋转的正确方法是什么？

//use transpose for clockwise rotation  
    Matrix mpYaw = Matrix.fromRotationZ(pYaw).getTranspose(); 
    Matrix mpPitch = Matrix.fromRotationY(pPitch).getTranspose(); 
    Matrix mpRoll = Matrix.fromRotationX(pRoll).getTranspose();  

    Matrix msYaw = Matrix.fromRotationZ(sYaw).getTranspose(); 
    Matrix msPitch = Matrix.fromRotationY(sPitch).getTranspose(); 
    Matrix msRoll = Matrix.fromRotationX(sRoll).getTranspose(); 

    Matrix mpRot = mpYaw.multiply(mpPitch).multiply(mpRoll); //platform  
    Matrix msRot = msYaw.multiply(msPitch).multiply(msRoll); //sensor 

    Matrix maRot = mpRot.multiply(msRot); //absolute 

样品MISB数据：

Platform Heading Angle:175.66308079652094 
Platform Pitch Angle:3.4296700949125647 
Platform Roll Angle:-0.3982665486617634  
Sensor Rel. Az. Angle:326.08593764856596 
Sensor Rel. El. Angle:-21.60937493741949 
Sensor Rel. Roll Angle:0.0 

Sensor Latitude:33.03482410173622 
Sensor Longitude:-114.45451377632772 
Sensor True Altitude:1022.4368657969026 
Frame Center Lat.:33.01531312661958 
Frame Center Lon.:-114.4367867216639 
Frame Center El.:79.58953231097883 
Slant Range:2883.640118614687 

编辑1：

通过@anjruu应用建议的修复后，它看起来像结果接近，但还是稍微偏离。我通过将旋转矩阵的正向矢量乘以MISB提供的目标距离来计算局部NED坐标到目标位置。然后，我计算了MISB提供的目标位置的本地NED坐标（使用ViewUtil），其中原点设置为提供的平台位置，结果稍微偏离。

Matrix mpYaw = Matrix.fromRotationZ(pYaw).getTranspose(); 
    Matrix mpPitch = Matrix.fromRotationY(pPitch).getTranspose(); 
    Matrix mpRoll = Matrix.fromRotationX(pRoll).getTranspose();  

    Matrix msYaw = Matrix.fromRotationZ(sYaw).getTranspose(); 
    Matrix msPitch = Matrix.fromRotationY(sPitch).getTranspose(); 
    Matrix msRoll = Matrix.fromRotationX(sRoll).getTranspose(); 

    Matrix mpRot = mpRoll.multiply(mpPitch).multiply(mpYaw); //platform  
    Matrix msRot = msRoll.multiply(msPitch).multiply(msYaw); //sensor 

    Matrix maRot = msRot.multiply(mpRot); //absolute 

    Globe globe = new Earth(); 

    Position pPlatform = Position.fromDegrees(33.03482410173622, -114.45451377632772, 1022.4368657969026); 
    Position pTarget = Position.fromDegrees(33.01531312661958, -114.4367867216639, 79.58953231097883); 
    double targetRange = 2883.640118614687; 

    Vec4 vTarNED = new Vec4(1,0,0).transformBy3(maRot.getTranspose()).multiply3(targetRange); 
    //NED = (-2165.935747907422, 1656.9597179630864, 937.3298046411029, 1.0) 

    Matrix localENU = ViewUtil.computePositionTransform(globe, pPlatform); 
    Vec4 vTarENU = globe.computePointFromPosition(pTarget).transformBy4(localENU); 
    //ENU = (1656.3846316600684, -2163.7501770820236, -943.4305881811306, 1.0) 
    //NED = (-2163.7501770820236, 1656.3846316600684, 943.4305881811306, 1.0) 

Answer 1

对于进一步的研究人员，我面临同样的问题。主要问题是传感器的误差率，要根据传感器数据直接设置视图位置和方向来计算这些误差并将它们添加为偏移值。但是，很可能我们有World Wind来为我们处理大部分计算。

随着使用任何3D引擎，你实际上没有任何给定的角度信息，因为你已经有了眼睛和lookAt位置。您可以从这些位置计算必要的方向值，也可以手动和自动进行管理。

在这里，我的功能根据给定的MISB KLV数据设置相机位置。

public void setCameraPosition(BTelemetryData pData){ 


    // Get Platform Location Information 
    Angle tPlatformLatitude = Angle.fromDegrees(Double.parseDouble(pData.getAlternatePlatformLatitude())); 
    Angle tPlatformLongitude = Angle.fromDegrees(Double.parseDouble(pData.getAlternatePlatformLongitude())); 
    double tPlatformAltitude = Double.parseDouble(pData.getPlatformGPSAltitude()); 
    Position tPlatfromPosition = new Position(tPlatformLatitude, tPlatformLongitude ,tPlatformAltitude); 

    // Get LookAt Location Information 
    Angle tLookAtLatitude = Angle.fromDegrees(Double.parseDouble(pData.getFrameCenterLatitude())); 
    Angle tLookAtLongitude= Angle.fromDegrees(Double.parseDouble(pData.getFrameCenterLongitude())); 
    // Note must take into account the surface elevation at given lat lon. 
    double tLookAtAltitude= getWwd().getModel().getGlobe().getElevation(tLookAtLatitude, tLookAtLongitude); 
    Position tLookAtPosition = new Position(tLookAtLatitude, tLookAtLongitude ,tLookAtAltitude); 

    // First things first, we need to Set Field of View 
    getView().setFieldOfView(Angle.fromDegrees(Double.parseDouble(pData.getSensorHorizontalFieldofView()))); 
    if (useAutoCameraPosition()) 
     setCameraPositionAutomatically(tLookAtPosition, tPlatfromPosition); 
    else 
     calculateAndSetCameraPosition(tLookAtPosition, tPlatfromPosition); 

    getView().firePropertyChange(AVKey.VIEW, null, getView()); 

} 

public void setCameraPositionAutomatically(Position pLookAtPosition, Position pEyePosition){ 
    getView().setEyePosition(pEyePosition); 
    getView().setOrientation(pEyePosition, pLookAtPosition); 
} 
public void calculateAndSetCameraPosition(Position pLookAtPosition, Position pEyePosition){ 
     double tPitch = getPitchAngleBetweenPositionInDegrees(pLookAtPosition, pEyePosition); 
     double tHeading = getHeadingAngleBetweenPositionInDegrees(pLookAtPosition, pEyePosition); 

     getView().setEyePosition(pEyePosition); 
     getView().setHeading(Angle.fromDegrees(tHeading)); 
     getView().setPitch(Angle.fromDegrees(tPitch)); 
} 


public double getPitchAngleBetweenPositionInDegrees(Position pLookAt, Position pEyePosition) { 

    // Calculate the radius at given look at position 
    double tRadius = getWwd().getModel().getGlobe().getRadiusAt(pLookAt); 

    // Find the Surrounding Radial Length Between those positions 
    double tRadialDistance = Position.greatCircleDistance(pLookAt, pEyePosition).getRadians() * tRadius; 

    // Find the Ratio Between Distance, which will give the offset and Angle 
    double tTheta = tRadialDistance/tRadius; 

    // Get the surface elevation of lookatposition 
    double tLookAtElevation = pLookAt.getElevation(); 

    // Get Altitude of given eye position 
    double tEyeAltitude = pEyePosition.getAltitude(); 

    // Delta Location Changes in cartesian 
    double tDeltaX = (tRadius + tLookAtElevation) * Math.cos(tTheta); 
    double tDeltaY = (tRadius + tLookAtElevation) * Math.sin(tTheta); 
    double tDeltaZ = tRadius + tEyeAltitude - tDeltaX; 

    double alpha = Math.atan(tDeltaZ/tDeltaY) - tTheta; 

    // Convert NED to World Wind Coordinate System. The Pitch angle should be 90 - calculated. 
    double degrees = 90 - Math.toDegrees(alpha); 

    System.out.println("Elevation Angle Between Positions = " + degrees); 
    return degrees; 
} 

public double getHeadingAngleBetweenPositionInDegrees(Position pLookAtPosition, Position pEyePosition) { 

    double tLatEye  = pEyePosition.getLatitude().radians; 
    double tLatLookAt = pLookAtPosition.getLatitude().radians; 

    double tLonLookAt = pLookAtPosition.getLongitude().radians; 
    double tLonEye = pEyePosition.getLongitude().radians; 
    double dLon = (tLonLookAt - tLonEye); 

    double y = Math.sin(dLon) * Math.cos(tLatLookAt); 
    double x = Math.cos(tLatEye) * Math.sin(tLatLookAt) - Math.sin(tLatEye) 
      * Math.cos(tLatLookAt) * Math.cos(dLon); 

    // Calculate the Bearing Angle. 
    double tBearing = Math.toDegrees(Math.atan2(y, x)); 

    // Calculate the absolute value of that Angle 
    tBearing = (tBearing + 360) % 360; 

    // Note that world wind takes the Heading in clockwise, if you want to make it counter clockwise, you need to subtract it from 360 degrees 
    //tBearing = 360 - tBearing; 

    return tBearing; 
} 

！ //

在我的代码中，我没有设置滚动角度，但根据文档，您可以简单地总结传感器和平面滚动角度，然后设置滚动角度。

有一点需要注意，World Wind有两个不同的视图类BasicOrbitView和BasicFlyView，为了模拟给定的数据你必须使用BasicFlyView。这是因为在FlyView中，当你设置角度时，你可以保持你的相机位置，但另一方面，在OrbitView中，你可以保持你的视角位置，并相对于这些角度改变角度和相机位置。如果精度对您来说足够好，您可以使用setOrientation方法。

The result:

具有良好的编码:)