Fortran 95 source code to convert WGS84(BLH) to ENU coordinate.

blh2enu.f95

!*******************************************************************************
! BLH -> ENU 変換
! : WGS84 の緯度(Beta)／経度(Lambda)／楕円体高(Height)を
!   ENU (East/North/Up; 地平) 座標に変換する。
!   * 途中、 ECEF（Earth Centered Earth Fixed; 地球中心・地球固定直交座標系）座標
!     への変換を経由。
!
!   date          name            version
!   2019.02.22    mk-mode.com     1.00 新規作成
!
! Copyright(C) 2019 mk-mode.com All Rights Reserved.
! ---
! 引数 :  BETA_O LAMBDA_O HEIGHT_O BETA LAMBDA HEIGHT
!         (BETA: Latitude, LAMBDA: Longitude)
!         (前半3つは原点、後半3つは対象)
!*******************************************************************************
!
program blh2enu
  implicit none
  ! SP: 単精度(4), DP: 倍精度(8)
  integer,      parameter :: SP = kind(1.0)
  integer(SP),  parameter :: DP = selected_real_kind(2 * precision(1.0_SP))
  character(*), parameter :: USAGE = "[USAGE] ./blh2enu BETA_O LAMBDA_O HEIGHT_O BETA LAMBDA HEIGHT"
  real(DP), parameter :: A      = 6378137.0_DP           ! a(地球楕円体長半径(赤道面平均半径))
  real(DP), parameter :: ONE_F  = 298.257223563_DP       ! 1 / f(地球楕円体扁平率=(a - b) / a)
  real(DP), parameter :: B      = A * (1.0_DP - 1.0_DP / ONE_F)  ! b(地球楕円体短半径)
  real(DP), parameter :: E2     = (1.0_DP / ONE_F) * (2.0_DP - (1.0_DP / ONE_F))
                                                         ! e^2 = 2 * f - f * f
                                                         !     = (a^2 - b^2) / a^2
  real(DP), parameter :: ED2    = E2 * A * A / (B * B)   ! e'^2= (a^2 - b^2) / b^2
  real(DP), parameter :: PI     = atan(1.0_DP) * 4.0_DP  ! 円周率
  real(DP), parameter :: PI_180 = PI / 180.0_DP          ! 0.0174532925199433
  real(DP) :: blh_o(3), blh(3), enu(3)  ! BLH 座標（原点、対象）, ENU 座標
  real(DP) :: az, el, dst               ! 方位角、仰角、距離
  real(DP) :: e_2, n_2, u_2             ! e^2, n^2, u^2 用

  ! コマンドライン引数取得
  call get_arg(blh_o, blh)
  if (blh_o(1) == 0) stop
  print '("BLH_O: LATITUDE(BETA) = ", F12.8, "°")', blh_o(1)
  print '("    LONGITUDE(LAMBDA) = ", F12.8, "°")', blh_o(2)
  print '("               HEIGHT = ", F12.3, "m" )', blh_o(3)
  print '("BLH:   LATITUDE(BETA) = ", F12.8, "°")', blh(1)
  print '("    LONGITUDE(LAMBDA) = ", F12.8, "°")', blh(2)
  print '("               HEIGHT = ", F12.3, "m" )', blh(3)

  ! BLH -> ENU
  call conv_blh2enu(blh_o, blh, enu)

  ! 方位角、仰角、距離
  az = atan2(enu(1), enu(2)) / PI_180
  if (az < 0.0_DP) az = az + 360.0_DP
  e_2 = enu(1) * enu(1)
  n_2 = enu(2) * enu(2)
  u_2 = enu(3) * enu(3)
  el = atan2(enu(3), sqrt(e_2 + n_2)) / PI_180
  dst = sqrt(e_2 + n_2 + u_2)

  ! 結果出力
  print '(A)', "--->"
  print '("ENU: E = ", F12.3, "m")',  enu(1)
  print '("     N = ", F12.3, "m")',  enu(2)
  print '("     U = ", F12.3, "m")',  enu(3)
  print '("方位角 = ", F12.3, "°")', az
  print '("  仰角 = ", F12.3, "°")', el
  print '("  距離 = ", F12.3, "°")', dst

  stop
contains
  ! コマンドライン引数取得
  !
  ! :param(out) real(8) blh_o(3): 原点 BLH 座標
  ! :param(out) real(8)   blh(3): 対象 BLH 座標
  subroutine get_arg(blh_o, blh)
    implicit none
    real(DP), intent(out) :: blh_o(3), blh(3)
    character(20) :: a

    blh_o = 0.0_DP
    blh   = 0.0_DP
    if (iargc() < 6) then
      print *, USAGE
      return
    end if
    call getarg(1, a)
    read (a, *) blh_o(1)
    call getarg(2, a)
    read (a, *) blh_o(2)
    call getarg(3, a)
    read (a, *) blh_o(3)
    call getarg(4, a)
    read (a, *) blh(1)
    call getarg(5, a)
    read (a, *) blh(2)
    call getarg(6, a)
    read (a, *) blh(3)
  end subroutine get_arg

  ! BLH -> ENU
  !
  ! :param(in)  real(8) blh_o(3): BLH 座標（原点）
  ! :param(in)  real(8)   blh(3): BLH 座標（対象）
  ! :param(out) real(8)   enu(3): ENU 座標
  subroutine conv_blh2enu(blh_o, blh, enu)
    implicit none
    real(DP), intent(in)  :: blh_o(3), blh(3)
    real(DP), intent(out) :: enu(3)
    real(DP) :: xyz_o(3), xyz(3)
    real(DP) :: mtx_0(3, 3), mtx_1(3, 3), mtx_2(3, 3), mtx_r(3, 3)

    call conv_blh2ecef(blh_o, xyz_o)
    call conv_blh2ecef(blh, xyz)
    call mtx_z(90.0_DP, mtx_0)
    call mtx_y(90.0_DP - blh_o(1), mtx_1)
    call mtx_z(blh_o(2), mtx_2)
    mtx_r = matmul(mtx_0, mtx_1)
    mtx_r = matmul(mtx_r, mtx_2)
    enu = matmul(mtx_r, xyz - xyz_o)
  end subroutine conv_blh2enu

  ! BLH -> ECEF
  !
  ! :param(in)  real(8) blh(3): BLH 座標
  ! :param(out) real(8) xyz(3): ECEF 座標
  subroutine conv_blh2ecef(blh, xyz)
    implicit none
    real(DP), intent(in)  :: blh(3)
    real(DP), intent(out) :: xyz(3)
    real(DP) :: c_lat, c_lon, s_lat, s_lon

    c_lat = cos(blh(1) * PI_180)
    c_lon = cos(blh(2) * PI_180)
    s_lat = sin(blh(1) * PI_180)
    s_lon = sin(blh(2) * PI_180)
    xyz = (/ &
      & (f(blh(1)) + blh(3)) * c_lat * c_lon,        &
      & (f(blh(1)) + blh(3)) * c_lat * s_lon,        &
      & (f(blh(1)) * (1.0_DP - E2) + blh(3)) * s_lat &
    & /)
  end subroutine conv_blh2ecef

  ! Function for blh2ecef
  !
  ! :param(in) real(8) x
  ! :return    real(8) f
  real(DP) function f(x)
    implicit none
    real(DP), intent(in) :: x
    real(DP) :: s

    s = sin(x * PI_180)
    f = A / sqrt(1.0_DP - E2 * s * s)
  end function f

  ! x 軸を軸とした回転行列
  !
  ! :param(in)  real(8)       ang: 回転量(°)
  ! :param(out) real(8) mtx(3, 3): 回転行列(3x3)
  subroutine mtx_x(ang, mtx)
    implicit none
    real(DP), intent(in) :: ang
    real(DP), intent(out) :: mtx(3, 3)
    real(DP) :: a, c, s

    a = ang * PI_180
    c = cos(a)
    s = sin(a)
    mtx= transpose(reshape( &
      & (/1.0_DP, 0.0_DP, 0.0_DP, 0.0_DP, c, s, 0.0_DP, -s, c/), &
      & (/3, 3/) &
    & ))
  end subroutine mtx_x

  ! y 軸を軸とした回転行列
  !
  ! :param(in)  real(8)       ang: 回転量(°)
  ! :param(out) real(8) mtx(3, 3): 回転行列(3x3)
  subroutine mtx_y(ang, mtx)
    implicit none
    real(DP), intent(in) :: ang
    real(DP), intent(out) :: mtx(3, 3)
    real(DP) :: a, c, s

    a = ang * PI_180
    c = cos(a)
    s = sin(a)
    mtx = transpose(reshape( &
      & (/c, 0.0_DP, -s, 0.0_DP, 1.0_DP, 0.0_DP, s, 0.0_DP, c/), &
      & (/3, 3/) &
    & ))
  end subroutine mtx_y

  ! z 軸を軸とした回転行列
  !
  ! :param(in)  real(8)       ang: 回転量(°)
  ! :param(out) real(8) mtx(3, 3): 回転行列(3x3)
  subroutine mtx_z(ang, mtx)
    implicit none
    real(DP), intent(in) :: ang
    real(DP), intent(out) :: mtx(3, 3)
    real(DP) :: a, c, s

    a = ang * PI_180
    c = cos(a)
    s = sin(a)
    mtx = transpose(reshape( &
      & (/c, s, 0.0_DP, -s, c, 0.0_DP, 0.0_DP, 0.0_DP, 1.0_DP/), &
      & (/3, 3/) &
    & ))
  end subroutine mtx_z
end program blh2enu