绝对传感器方向

Question

我正在使用来自MISB KLV Local Dataset 的数据和NASA Worldwind 中提供的矩阵运算。 MISB 数据以 Yaw、Pitch 和 Roll 提供平台方向以及在 Yaw、Pitch 和 Roll 中相对于平台的传感器方向。我正在尝试根据平台方向和传感器相对方向计算传感器相对于北方的绝对方向（偏航、俯仰、滚动）。

我目前正在计算平台旋转矩阵和传感器相对旋转矩阵，并将结果相乘。生成的旋转矩阵似乎不正确。根据第 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 引擎时，您实际上并没有任何给定的角度信息，因为您已经拥有眼睛和观察位置。您可以从这些位置计算必要的方向值，也可以手动和自动管理它。

在这里，在我的函数中，根据给定的 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:

良好的编码:)