早安，苦命工程師的胡言亂語

前言：

之前對雷達的運作跟如何得到待側物的目標是完全沒有概念的。後來因為工作上的需要，上網找了一下資料，才發現，這蠻簡單的嘛！只要透過地平線的座標系統，知道待側物的仰角 EL，方位角 Az，及雷達站所量測到的待側物跟雷達之間的距離 Range，這樣待側物的座標就立刻可以被計算出來了。(這其實是一種球座標系統轉直角座標系統的轉換)

NASA 利用這樣的方式，他們在美國加州的 Edwards 有個叫做 Dryden Flight Research Center (FRC) 的雷達站，透過這個雷達，他們可以隨時追蹤目前太空梭的位置，並立即的提供給太空梭的任務中心的 Flight Dynamics Officer (FDO) 來監控，並計算整個太空梭任務的飛行軌道。

來推導並瞭解一下，這整個座標轉換中，所需要的數學公式吧！

因為公式是自己推導的，所以順便做個心得筆記！

座標系統：

1. ECEF 座標系統

地球上的位置，因為地球是個橢圓般球體，所以比較適合的描述系統是球座標的經度 (Longitude)，緯度 (Latitude)，及高度 (Height) 來描述地球上任一處的位置。

可是實際上，經度跟緯度是不太容易做計算的，比較容易計算還是得使用直角笛卡爾座標系統才是。

為了方便計算，我們可以採用一種以地球中心為原點，隨著地球自轉公轉一起移動的地球中心固定座標系統(Earth Centered Earth Fixed, ECEF)。方便描述地球上物體的所有位置跟相對運動。

NASA 為了方便計算，一律都採用這樣的ECEF的直角坐標系統，並稱之為 Space Vector，然後用 i, j, k 的單位向量來標注它們。

2. ENU 導航座標系統

對於在地球上生活的我們，我們並不會覺得我們生活在一顆球體的表面。而是感覺生活在一個巨大的平面上。這個平面，我們可以感受到地球北邊的方向 North，地球南邊的方向 South。東邊的方向 East，西邊的方向 West。往上 Up，往下 Down。這樣也是一個巨大的直角座標系統。

只是這個直角座標系統並不是靜止固定的，而是由觀察者在地球上的座標位置(經緯度)而決定的，永遠跟地球的這個球體表面相切。如果， East 的方向為正，North 的方向為正， Up 的方向為正。這樣就被稱為 ENU 的直角座標系統。

因為這個座標系統也最常被使用在汽車或飛機的導航，所以也有人喜歡稱它為 NAV 導航座標系統。

3. 地平線座標系統 (Horizontal Coordinate System)

利用方位角Azimuth, Az 及高度角 Elevation, EL來標定，最適合用來描述天空中物體的位置。例如天空中星星的位置，跟太陽月亮的位置。只要給定方位角 Az, 及高度角 EL，它們的位置就確定了，如果再給上此物體跟你的相對距離 range，它在三度空間中的位置就完全確定。

基本上，跟經緯度一樣，這是一種球座標系統。

方位角 Azimuth, Az: 物體投影到 x-y 平面時，投影線跟 North 軸之間的夾角。(以North 軸當角度的起點)

高度角 Elevation, EL: 物體跟地平線之間的夾角 (這是地平線座標系統名稱的由來。

距離. Range: 物體跟座標原點(觀察者)之間的距離。

經緯度座標跟 ECEF 座標之間的轉換

來推導公式吧，

給定一個地球上的一點經緯度座標，經度 Long (Longitude)，緯度 Lat (Latitude) 跟它跟地球地心距離R。把它轉到 ECEF 座標系，此時的轉換公式為何?

非常直覺跟簡單的推導，R投影到 x-y (i-j) 平面的大小為 R*cos(Lat)

所以顯然的，

x = R*cos(Lat) * cos(Long),  R*cos(Lat) 在 x軸上的投影

y = R*cos(Lat) * sin(Long),  R*cos(Lat) 在 y軸上的投影

另外，很顯然的，R 在 z軸的投影為

z = R*sin(Lat)

NASA 雷達站 FRC 的 ECEF 座標

前言有提過， NASA 把 ECEF 座標系稱之為 Space Vector，所有的量測計算會先轉換到這個座標系統後才會一起做計算。

底下是 NASA 在美國加州的Dryden Flight Research Center (FRC) 的雷達站的經緯度座標，請問這個雷達站的 Space Vector (ECEF 座標系的位置)是多少呢？

簡單，套上剛剛我們推導的公式吧

x = R cos(Lat) cos(Long) = 6378.889 cos(34.9607796) cos(242.0885039) = -2447.163 km

y = R cos(Lat) sin(Long) = 6378.889 cos(34.9607796) sin(242.0885039) = -4619.644 km

z = R sin(Lat) = 6378.889 sin(34.9607796) = 3655.203 km

所以，FRC雷達站在 ECEF 座標中的位置為

Fec = -2447.163 i -4619.644 j + 3655.203 k

ENU導航座標跟 ECEF 座標之間的轉換

前面有說過的， ENU導航座標是變動的，不是固定的。隨著地球表面不同的位置不一樣而不一樣，而且永遠相切於地球球體的表面。

所以，當談到 ENU導航座標時，需要先給出這個 ENU導航座標到底是位於地球表面的何處，這樣才能確立整個 ENU座標系。亦即，你必須先指出這個 ENU座標系位於地球上的經緯度。

讓我們來推導並找出來，假如在 ENU 座標系上有一個向量 r = (re, rn, ru) = re e + rn n + ru u

那如何轉換至 ECEF 座標系呢？座標轉換公式是什麼呢?

ENU導航座標和經緯度 Long/Lat 之間的關係

要找出座標轉換關係式，需要從單位向量來著手。只要把單位向量 e, n, u 分解至 ECEF 座標的 i, j, k 分量，這樣轉換式就可以確立囉！

先從 n 來著手，

向量圖畫一畫，可以發現，單位向量 n 投影在 i-j (x-y) 平面的向量圖如下，

投影過來的大小為 sin(Lat)，然後方向為 -i, -j

所以，立刻得到答案

再來對 e 來著手，

一樣，向量圖畫一畫，可以發現，單位向量 e 投影在 i-j (x-y) 平面的向量圖如下，

e 必須跟地球的緯度圓相切，

所以，也是立刻得到答案

最後，對 u 著手，

u 是 e-n 平面的法向量

所以，也是立刻得到答案

最後得到完整的座標轉換式

轉成比較好操作的矩陣形式，我們也得到我們所要的 ENU 座標轉換至 ECEF 座標的旋轉矩陣

雷達偵測原理和地平座標

那， NASA 的地面雷達站是如何偵測太空梭的位置的呢？

原理如下面的示意圖啊，雷達天線以特定的方位角 Az，及仰角 El ，終於抓到太空梭的回波了。根據回波的時間差，可以算出太空梭跟雷達站之間的距離 range

所以一次成功的量測，可以得到太空梭的球座標位置 (range, El, Az)

老戲法，先投影到 e-n 平面，很快的可以寫出座標轉換

將雷達抓到的太空梭位置轉換成 ENU 座標系

我們的目標是 ECEF 座標系，所以來做一次 ENU 轉 ECEF 的座標轉換

把結果跟 ENU轉ECEF 轉換矩陣做個矩陣相乘

終於，費了好大的勁。

上面就是我們最終要的結果。我們把雷達量測的結果轉成ECEF座標下的 Space Vector。

問題：

在一次太空梭太空任務中，NASA在美國加州的 FRC 雷達站測量到太空梭的位置數據如下，請問太空梭在 ECEF 的實際座標位置為何？

FRC 的經緯度

解答：

根據我們所推出來的最終公式，

x = 505.6889904*(-sin(242.0885039)*cos(40.8300297)*sin(199.9850926)-sin(34.9607796)*cos(242.0885039)*cos(40.8300297)*cos(199.9850926)+cos(34.9607796)*cos(242.0885039)*sin(40.8300297))

x = -338.855 km

y = 505.6889904*(cos(242.0885039)*cos(40.8300297)*sin(199.9850926)-sin(34.9607796)*sin(242.0885039)*cos(40.8300297)*cos(199.9850926)+cos(34.9607796)*sin(242.0885039)*sin(40.8300297))

y = -360.309 km

z = 505.6889904*(cos(34.9607796)*cos(40.8300297)*cos(199.9850926)+sin(34.9607796)*sin(40.8300297))

z = -105.245 km

所以

Sfc = -338.855 i -360.309 j -105.245 k

要注意的喲，我們的座標轉換只是座標的旋轉而已，並沒有考慮平移的喲！

所以上面 Sfc 的向量依舊是錯的，是以雷達站 FRC 為原點的。所以我們必須把 FRC 的座標再加上去，才會是正確 ECEF 的座標。

之前，我們算過 雷達站 FRC 的座標是 Fec = -2447.163 i -4619.644 j + 3655.203 k

所以真正太空梭在ECEF座標的位置為

Sec = Fec + Sfc

= (-2447.163 i -4619.644 j + 3655.203 k) + (-338.855 i -360.309 j -105.245 k)

= -2786.018 i -4979.953 j +3549.958 k

問題：

地球的半徑是 6378.137 km，所以請問此時太空梭距離地球表面有多高呢?

解答：

Sec = -2786.018 i -4979.953 j +3549.958 k

這是 ECEF 座標，以地球中心為原點。所以這個太空梭位置向量的大小減去地球半徑就是太空梭距離地球的高度囉！

h = sqrt(x^2 + y^2 + z^2) - R.earth

h = sqrt((-2786.018)^2+(-4979.953)^2+3549.958^2) - 6378.137

  = 342.282 km

所以太空梭此時正距離地球表面 342.282 km 的高度！

附記：

2022/8/7

補上一些測試關於 ENU 轉 ECEF 的轉換矩陣。

對一個旋轉矩陣而言，數學上可以證明出它們有一個重要的特性，如果它的反矩陣等於轉置矩陣的話，我們說它是正交的。

那我們這個 ENU 轉 ECEF 的轉換矩陣到底是不是旋轉矩陣，滿足反矩陣等於轉置矩陣的這個特性的呢。

當然可以直接用正交公式來計算跟證明啦。但是筆者懶得去翻書找旋轉矩陣的正交條件囉。

這裡用另外一種方式，用 FORTH 寫個小程式，將ENU 轉 ECEF 的變換矩陣根據經緯度設定好，然後用高斯喬登法將此變換矩陣的反矩陣算出來，最後來驗證看看是不是等於它的轉置矩陣。

如果是的話，就證明囉ENU 轉 ECEF 的變換矩陣也是一種旋轉矩陣囉。

以 Latitude: 34.9607796, Longitude: 242.0885039 來進行計算，結果如下

結果是很確定的，

確實反矩陣等於轉置矩陣，滿足旋轉矩陣所必須具備的正交條件。

所以逆變換等於轉置，可以立刻被寫出來而無需計算反矩陣。

FORTH 程式列表

\
\ ECEF to ENU matrix with its invert matrix verify
\   Frank Lin 2022.8.7
\

: DefMatrix
   create  ( RowNum ColNum --)
     2dup , ,  * floats allot   
;

: ColNum@  ( maddr -- ColNum)
   @
;

: RowNum@  ( maddr -- RowNum)
  cell+ @
;

: []  ( Row Col maddr -- addr')
   >r r@ @   ( Row Col ColNum | maddr)
   rot *     ( Col ColNum*Row | maddr)
   +  floats  [ 2 cells ] literal +
   r> +
;

: .mat ( maddr --)
    cr
    dup RowNum@   0
    do   dup ColNum@ 0
      do    dup    j i rot [] f@ f.  space
      loop cr
    loop drop
;

: EmptyMatrix ( maddr --)
   dup @                 ( maddr ColNum)
   swap cell+ dup @      ( ColNum maddr' RowNum)
   swap cell+ -rot   * floats
   erase
;

\ defer define for Matrix

: DummyMatrix
   0 value
;

\  Ex:
\  DummyMatrix MatSelected
\
\  4 5 DefMatrix ExperimentData
\  ExperimentData to MatSelected
\

DummyMatrix MatSelected

: RowScalar* ( f.scalar row --)
   MatSelected  ColNum@   0
   do    dup i MatSelected []   dup f@  fover f*   f!
   loop  drop
;

: RowUnitizebyCol ( Row Col --)
   2dup MatSelected [] f@  ( scalar)  fdup f0<>
   if   1e0 fswap f/   drop RowScalar*  
   else 2drop then
;

: Row2Row1Scalar*Subtract>Row2 ( Row2 Row1 f.scalar --)
   MatSelected  ColNum@   0
   do  over i MatSelected [] dup f@  ( r2 r1 r2adr f.sc f.r2)
       fover                         ( r2 r1 r2adr f.sc f.r2 f.sc)
       over i MatSelected [] f@  f*  ( r2 r1 r2adr f.sc f.r2 f.r1*sc)   
       f-   f!
   loop  2drop fdrop
;

: LowerTrianglize
   MatSelected RowNum@ 0
   do
       MatSelected RowNum@ i
       do  
           i j MatSelected [] f@ f0<>
           if
              i j RowUnitizeByCol
              i j =
              if   j j MatSelected [] f@   f0= 
                   abort" Pivot is ZERO!!"
              else i j 1e0 
                   Row2Row1Scalar*Subtract>Row2   
              then
           then
       loop
   loop
;

: UpperTrianglize
   MatSelected RowNum@ 1
   do
       0 i
       do  
           i j MatSelected [] f@ f0<>
           if
              i j <>
              if  i j   2dup MatSelected [] f@   
                  Row2Row1Scalar*Subtract>Row2   
              then
           then
       -1
       +loop
   loop
;

: RowSwap ( row2 row1 --)
   2dup = if 2drop exit then
   MatSelected ColNum@  0
   do over i MatSelected [] dup f@  ( r2 r1 r2adr f.r2)
      over i MatSelected [] dup f@  ( r2 r1 r2adr r1adr f.r2 f.r1)
      fswap  f! f!
   loop  2drop
;

: AdjustPivot ( --)
   MatSelected RowNum@  0
   do i   i i MatSelected [] f@ fabs ( ColMax f.max)
      ( search for max pivot)
      MatSelected RowNum@  i
      do  i j MatSelected [] f@ fabs ( ColMax f.max f.new)
          fover fover f< 
          if  fswap   drop i   then
          fdrop
      loop
      ( swap with max pivot)
      fdrop  i RowSwap
   loop
;

: SolveLinearEQ ( m.addr --)
   to MatSelected
   AdjustPivot
   LowerTrianglize
   UpperTrianglize
;

fvariable lati
fvariable longi

3 6 defMatrix RotMatrix

: MatrixSet ( F: lati longi --)
   pi f*  180e0 f/   longi  f!
   pi f*  180e0 f/   lati   f!
   
   longi f@ fsin                     fnegate    0 0 RotMatrix [] f!
   lati  f@ fsin  longi f@ fcos  f*  fnegate    0 1 RotMatrix [] f!
   lati  f@ fcos  longi f@ fcos  f*             0 2 RotMatrix [] f!
   1e0                                          0 3 RotMatrix [] f!
   0e0                                          0 4 RotMatrix [] f!
   0e0                                          0 5 RotMatrix [] f!
   
   longi f@ fcos                                1 0 RotMatrix [] f!
   lati  f@ fsin  longi f@ fsin  f*  fnegate    1 1 RotMatrix [] f!
   lati  f@ fcos  longi f@ fsin  f*             1 2 RotMatrix [] f!
   0e0                                          1 3 RotMatrix [] f!
   1e0                                          1 4 RotMatrix [] f!
   0e0                                          1 5 RotMatrix [] f!
   
   0e0                                          2 0 RotMatrix [] f!
   lati  f@ fcos                                2 1 RotMatrix [] f!
   lati  f@ fsin                                2 2 RotMatrix [] f!
   0e0                                          2 3 RotMatrix [] f!
   0e0                                          2 4 RotMatrix [] f!
   1e0                                          2 5 RotMatrix [] f!   
;

6 set-precision

cr
.( test with Latitude: 34.9607796, Longitude: 242.0885039) cr

   
34.9607796e0 242.0885039e0 MatrixSet

.( it's content:) cr
RotMatrix .mat

cr
.( calculating inverse Matrix...) cr

RotMatrix SolveLinearEQ

cr
.( Inverse Matrix:)
RotMatrix .mat cr

xxx