coordinate frames transformation
This commit is contained in:
nuknal
@@ -38,13 +38,13 @@ func CameraDirectionVec(u, v float64) []float64 {
|
||||
// fmt.Printf("Diagonal (calculated from FOV): %.6f mm\n", dCalcOfFOV)
|
||||
|
||||
directionVec := []float64{0, 0, 0}
|
||||
directionVec[0] = 0 // x方向, mm，线性CCD每次单行成像
|
||||
directionVec[1] = (v - PANPixels/2) * PANCellSize / 1000.0 // y方向, mm
|
||||
directionVec[2] = -FocalLength // z方向, mm
|
||||
directionVec[0] = 0 // x方向, mm，线性CCD每次单行成像
|
||||
directionVec[1] = (v - PANPixels/2) * PANCellSize / 1000.0 / 1000.0 // y方向, mm
|
||||
directionVec[2] = -FocalLength / 1000.0 // z方向, mm
|
||||
|
||||
// 归一化
|
||||
// fmt.Printf("Direction Vector: (%.6f, %.6f, %.6f) \n", directionVec[0], directionVec[1], directionVec[2])
|
||||
directionVec = normalizeVec(directionVec)
|
||||
// directionVec = normalizeVec(directionVec)
|
||||
// fmt.Printf("Direction Vector (normalized): (%.6f, %.6f, %.6f) \n", directionVec[0], directionVec[1], directionVec[2])
|
||||
|
||||
return directionVec
|
||||
|
||||
266
pkg/calculator/eop.go
Normal file
266
pkg/calculator/eop.go
Normal file
@@ -0,0 +1,266 @@
|
||||
package calculator
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"bytes"
|
||||
_ "embed"
|
||||
"fmt"
|
||||
"io"
|
||||
"math"
|
||||
"os"
|
||||
"strconv"
|
||||
"strings"
|
||||
"time"
|
||||
|
||||
"github.com/hebl/gofa"
|
||||
)
|
||||
|
||||
const (
|
||||
ARC_SECONDS_PER_RADIAN = 206264.80624709636
|
||||
)
|
||||
|
||||
// https://www.iers.org/IERS/EN/DataProducts/EarthOrientationData/eop.html
|
||||
// https://maia.usno.navy.mil/ser7/finals2000A.all
|
||||
type EOPEntry struct {
|
||||
Year int
|
||||
Month int
|
||||
Day int
|
||||
MJD float64
|
||||
Xp float64 // 弧度 rad
|
||||
Yp float64 // 弧度 rad
|
||||
Dut1 float64
|
||||
}
|
||||
|
||||
// https://earth-info.nga.mil/#eopp
|
||||
type EOPP5Line struct {
|
||||
Ta float64
|
||||
A, B float64
|
||||
C, D, P [2]float64
|
||||
E, F float64
|
||||
G, H, Q [2]float64
|
||||
Tb float64
|
||||
I, J float64
|
||||
K, L, R [4]float64
|
||||
}
|
||||
|
||||
type EOPTable struct {
|
||||
URL string
|
||||
Local2000A string
|
||||
EOPs map[float64]*EOPEntry
|
||||
EOPP5Line EOPP5Line
|
||||
UseEOPP5Line bool
|
||||
}
|
||||
|
||||
var EOP *EOPTable
|
||||
|
||||
func NewEOPTable() *EOPTable {
|
||||
eop := &EOPTable{
|
||||
URL: "https://maia.usno.navy.mil/ser7/finals2000A.all",
|
||||
Local2000A: "data/EOP/finals2000A.all",
|
||||
EOPs: make(map[float64]*EOPEntry),
|
||||
UseEOPP5Line: false, // 当天使用 EOPP5Line 预测结果无法通过测试
|
||||
}
|
||||
return eop
|
||||
}
|
||||
|
||||
func (eop *EOPTable) Load(eopData []byte, eopp5Line []byte) error {
|
||||
f, err := os.Open(eop.Local2000A)
|
||||
if err != nil {
|
||||
return eop.loadEmbed(eopData, eopp5Line)
|
||||
}
|
||||
defer f.Close()
|
||||
|
||||
data, err := io.ReadAll(f)
|
||||
if err != nil {
|
||||
return eop.loadEmbed(eopData, eopp5Line)
|
||||
}
|
||||
|
||||
return eop.loadEmbed(data, eopp5Line)
|
||||
}
|
||||
|
||||
func (eop *EOPTable) loadEmbed(eopData []byte, eopp5Line []byte) error {
|
||||
reader := bufio.NewReader(bytes.NewReader(eopData))
|
||||
for {
|
||||
line, _, err := reader.ReadLine()
|
||||
if err == io.EOF {
|
||||
break
|
||||
}
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
var e EOPEntry
|
||||
if err := e.line2EOPEntry(line); err != nil {
|
||||
break
|
||||
}
|
||||
|
||||
eop.EOPs[e.MJD] = &e
|
||||
}
|
||||
|
||||
eop.EOPP5Line.parse(eopp5Line)
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func (eop EOPTable) Get(mjd float64) (*EOPEntry, bool) {
|
||||
e, ok := eop.EOPs[mjd]
|
||||
if !ok && eop.UseEOPP5Line {
|
||||
xp, yp, dut1 := eop.EOPP5Line.Predict(mjd)
|
||||
e = &EOPEntry{
|
||||
MJD: mjd,
|
||||
Xp: xp / ARC_SECONDS_PER_RADIAN,
|
||||
Yp: yp / ARC_SECONDS_PER_RADIAN,
|
||||
Dut1: dut1,
|
||||
}
|
||||
ok = true
|
||||
}
|
||||
return e, ok
|
||||
}
|
||||
|
||||
func (eop EOPTable) GetByUTCTime(utctime time.Time) (*EOPEntry, bool) {
|
||||
var djm0, mjd float64
|
||||
gofa.Cal2jd(int(utctime.Year()), int(utctime.Month()), int(utctime.Day()), &djm0, &mjd)
|
||||
return eop.Get(mjd)
|
||||
}
|
||||
|
||||
// https://maia.usno.navy.mil/ser7/readme.finals2000A
|
||||
// The format of the finals2000A.data, finals2000A.daily, and finals2000A.all files is:
|
||||
|
||||
// Col.# Format Quantity
|
||||
// ------- ------ -------------------------------------------------------------
|
||||
// 1-2 I2 year (to get true calendar year, add 1900 for MJD<=51543 or add 2000 for MJD>=51544)
|
||||
// 3-4 I2 month number
|
||||
// 5-6 I2 day of month
|
||||
// 7 X [blank]
|
||||
// 8-15 F8.2 fractional Modified Julian Date (MJD UTC)
|
||||
// 16 X [blank]
|
||||
// 17 A1 IERS (I) or Prediction (P) flag for Bull. A polar motion values
|
||||
// 18 X [blank]
|
||||
// 19-27 F9.6 Bull. A PM-x (sec. of arc)
|
||||
// 28-36 F9.6 error in PM-x (sec. of arc)
|
||||
// 37 X [blank]
|
||||
// 38-46 F9.6 Bull. A PM-y (sec. of arc)
|
||||
// 47-55 F9.6 error in PM-y (sec. of arc)
|
||||
// 56-57 2X [blanks]
|
||||
// 58 A1 IERS (I) or Prediction (P) flag for Bull. A UT1-UTC values
|
||||
// 59-68 F10.7 Bull. A UT1-UTC (sec. of time)
|
||||
// 69-78 F10.7 error in UT1-UTC (sec. of time)
|
||||
// 79 X [blank]
|
||||
// 80-86 F7.4 Bull. A LOD (msec. of time) -- NOT ALWAYS FILLED
|
||||
// 87-93 F7.4 error in LOD (msec. of time) -- NOT ALWAYS FILLED
|
||||
// 94-95 2X [blanks]
|
||||
// 96 A1 IERS (I) or Prediction (P) flag for Bull. A nutation values
|
||||
// 97 X [blank]
|
||||
// 98-106 F9.3 Bull. A dX wrt IAU2000A Nutation (msec. of arc), Free Core Nutation NOT Removed
|
||||
// 107-115 F9.3 error in dX (msec. of arc)
|
||||
// 116 X [blank]
|
||||
// 117-125 F9.3 Bull. A dY wrt IAU2000A Nutation (msec. of arc), Free Core Nutation NOT Removed
|
||||
// 126-134 F9.3 error in dY (msec. of arc)
|
||||
// 135-144 F10.6 Bull. B PM-x (sec. of arc)
|
||||
// 145-154 F10.6 Bull. B PM-y (sec. of arc)
|
||||
// 155-165 F11.7 Bull. B UT1-UTC (sec. of time)
|
||||
// 166-175 F10.3 Bull. B dX wrt IAU2000A Nutation (msec. of arc)
|
||||
// 176-185 F10.3 Bull. B dY wrt IAU2000A Nutation (msec. of arc)
|
||||
func (e *EOPEntry) line2EOPEntry(line []byte) error {
|
||||
if len(line) < 68 {
|
||||
return fmt.Errorf("invalid eop line")
|
||||
}
|
||||
|
||||
e.Year, _ = strconv.Atoi(string(line[0:2]))
|
||||
e.Month, _ = strconv.Atoi(strings.TrimSpace(string(line[2:4])))
|
||||
e.Day, _ = strconv.Atoi(strings.TrimSpace(string(line[4:6])))
|
||||
e.MJD, _ = strconv.ParseFloat(string(line[7:15]), 64)
|
||||
|
||||
e.Xp, _ = strconv.ParseFloat(strings.TrimSpace(string(line[18:27])), 64)
|
||||
e.Yp, _ = strconv.ParseFloat(strings.TrimSpace(string(line[37:46])), 64)
|
||||
|
||||
e.Xp = e.Xp / ARC_SECONDS_PER_RADIAN
|
||||
e.Yp = e.Yp / ARC_SECONDS_PER_RADIAN
|
||||
|
||||
e.Dut1, _ = strconv.ParseFloat(strings.TrimSpace(string(line[58:68])), 64)
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func (e *EOPP5Line) parse(eopp5Line []byte) error {
|
||||
e.P[0] = 1.0
|
||||
e.P[1] = 1.0
|
||||
for i := 0; i < 4; i++ {
|
||||
e.R[i] = 1.0
|
||||
}
|
||||
|
||||
reader := bufio.NewReader(bytes.NewReader(eopp5Line))
|
||||
var lines [][]byte
|
||||
for {
|
||||
line, _, err := reader.ReadLine()
|
||||
if err == io.EOF {
|
||||
break
|
||||
}
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
lines = append(lines, line)
|
||||
}
|
||||
|
||||
if len(lines) < 5 {
|
||||
return fmt.Errorf("invalid eopp5 lines")
|
||||
}
|
||||
|
||||
e.Ta, _ = strconv.ParseFloat(strings.TrimSpace(string(lines[0][0:10])), 64)
|
||||
e.A, _ = strconv.ParseFloat(strings.TrimSpace(string(lines[0][10:20])), 64)
|
||||
e.B, _ = strconv.ParseFloat(strings.TrimSpace(string(lines[0][20:30])), 64)
|
||||
e.C[0], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[0][30:40])), 64)
|
||||
e.C[1], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[0][40:50])), 64)
|
||||
e.D[0], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[0][50:60])), 64)
|
||||
e.D[1], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[0][60:70])), 64)
|
||||
e.P[0], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[0][70:76])), 64)
|
||||
|
||||
e.P[1], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[1][0:6])), 64)
|
||||
e.E, _ = strconv.ParseFloat(strings.TrimSpace(string(lines[1][6:16])), 64)
|
||||
e.F, _ = strconv.ParseFloat(strings.TrimSpace(string(lines[1][16:26])), 64)
|
||||
e.G[0], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[1][26:36])), 64)
|
||||
e.G[1], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[1][36:46])), 64)
|
||||
e.H[0], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[1][46:56])), 64)
|
||||
e.H[1], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[1][56:66])), 64)
|
||||
e.Q[0], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[1][66:72])), 64)
|
||||
e.Q[1], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[1][72:78])), 64)
|
||||
|
||||
e.Tb, _ = strconv.ParseFloat(strings.TrimSpace(string(lines[2][0:10])), 64)
|
||||
e.I, _ = strconv.ParseFloat(strings.TrimSpace(string(lines[2][10:20])), 64)
|
||||
e.J, _ = strconv.ParseFloat(strings.TrimSpace(string(lines[2][20:30])), 64)
|
||||
e.K[0], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[2][30:40])), 64)
|
||||
e.K[1], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[2][40:50])), 64)
|
||||
e.K[2], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[2][50:60])), 64)
|
||||
e.K[3], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[2][60:70])), 64)
|
||||
|
||||
e.L[0], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[3][0:10])), 64)
|
||||
e.L[1], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[3][10:20])), 64)
|
||||
e.L[2], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[3][20:30])), 64)
|
||||
e.L[3], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[3][30:40])), 64)
|
||||
e.R[0], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[3][40:49])), 64)
|
||||
e.R[1], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[3][49:58])), 64)
|
||||
e.R[2], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[3][58:67])), 64)
|
||||
e.R[3], _ = strconv.ParseFloat(strings.TrimSpace(string(lines[3][67:76])), 64)
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
// https://earth-info.nga.mil/php/download.php?file=gnss-eopp
|
||||
func (e *EOPP5Line) Predict(mjd float64) (xp, yp, dut1 float64) {
|
||||
xp = e.A + e.B*(mjd-e.Ta) +
|
||||
e.C[0]*math.Sin(2*math.Pi*(mjd-e.Ta)/e.P[0]) + e.D[0]*math.Cos(2*math.Pi*(mjd-e.Ta)/e.P[0]) +
|
||||
e.C[1]*math.Sin(2*math.Pi*(mjd-e.Ta)/e.P[1]) + e.D[1]*math.Cos(2*math.Pi*(mjd-e.Ta)/e.P[1])
|
||||
|
||||
yp = e.E + e.F*(mjd-e.Ta) +
|
||||
e.G[0]*math.Sin(2*math.Pi*(mjd-e.Ta)/e.Q[0]) + e.H[0]*math.Cos(2*math.Pi*(mjd-e.Ta)/e.Q[0]) +
|
||||
e.G[1]*math.Sin(2*math.Pi*(mjd-e.Ta)/e.Q[1]) + e.H[1]*math.Cos(2*math.Pi*(mjd-e.Ta)/e.Q[1])
|
||||
|
||||
dut1 = e.I + e.J*(mjd-e.Tb) +
|
||||
e.K[0]*math.Sin(2*math.Pi*(mjd-e.Tb)/e.R[0]) + e.L[0]*math.Cos(2*math.Pi*(mjd-e.Tb)/e.R[0]) +
|
||||
e.K[1]*math.Sin(2*math.Pi*(mjd-e.Tb)/e.R[1]) + e.L[1]*math.Cos(2*math.Pi*(mjd-e.Tb)/e.R[1]) +
|
||||
e.K[2]*math.Sin(2*math.Pi*(mjd-e.Tb)/e.R[2]) + e.L[2]*math.Cos(2*math.Pi*(mjd-e.Tb)/e.R[2]) +
|
||||
e.K[3]*math.Sin(2*math.Pi*(mjd-e.Tb)/e.R[3]) + e.L[3]*math.Cos(2*math.Pi*(mjd-e.Tb)/e.R[3])
|
||||
|
||||
return
|
||||
}
|
||||
@@ -15,7 +15,7 @@ type IntersectionPoint struct {
|
||||
H float64
|
||||
}
|
||||
|
||||
func IntersectionAttitude(q Quaternion, satPos84 []float64, satTime time.Time, ucam int) (IntersectionPoint, error) {
|
||||
func IntersectionAttitude(q Quaternion, satPosECI, satPos84 []float64, satTime time.Time, ucam int) (IntersectionPoint, error) {
|
||||
// alpha := FOV * math.Pi / 180.0
|
||||
// alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels))
|
||||
// direction := []float64{0, math.Tan(alpha), -1.3}
|
||||
@@ -33,16 +33,19 @@ func IntersectionAttitude(q Quaternion, satPos84 []float64, satTime time.Time, u
|
||||
dECI := result.RawVector().Data
|
||||
|
||||
// -------- 转到ECEF坐标系 --------
|
||||
x, y, z := ECItoECEF(dECI[0], dECI[1], dECI[2], satTime)
|
||||
dECEF := []float64{x, y, z}
|
||||
// x, y, z := ECItoECEF(dECI[0], dECI[1], dECI[2], satTime)
|
||||
// dECEF := []float64{x, y, z}
|
||||
|
||||
// -------- 计算与地球表面的交点 --------
|
||||
intersection, err := intersectWithEllipsoid(satPos84, dECEF)
|
||||
intersection, err := intersectWithEllipsoid(satPosECI, dECI)
|
||||
if err != nil {
|
||||
return IntersectionPoint{}, err
|
||||
}
|
||||
|
||||
lat, lon, h := ECEFToGeodetic(intersection[0], intersection[1], intersection[2])
|
||||
x, y, z := ECItoECEF(intersection[0], intersection[1], intersection[2], satTime)
|
||||
intersection = []float64{x, y, z}
|
||||
|
||||
lat, lon, h := ECEFGeocentricToGeodetic(intersection[0], intersection[1], intersection[2])
|
||||
return IntersectionPoint{Lat: lat, Lon: lon, H: h}, nil
|
||||
}
|
||||
|
||||
@@ -78,7 +81,7 @@ func IntersectionECI(Qsat2orbit, Qorbit2eci Quaternion, satPos84 []float64, satT
|
||||
return IntersectionPoint{}, err
|
||||
}
|
||||
|
||||
lat, lon, h := ECEFToGeodetic(intersection[0], intersection[1], intersection[2])
|
||||
lat, lon, h := ECEFGeocentricToGeodetic(intersection[0], intersection[1], intersection[2])
|
||||
|
||||
return IntersectionPoint{Lat: lat, Lon: lon, H: h}, nil
|
||||
}
|
||||
|
||||
@@ -9,11 +9,11 @@ import (
|
||||
func J2000ToWGS84(j2000X, j2000Y, j2000Z float64, utc time.Time) (float64, float64, float64) {
|
||||
itrsX, itrsY, itrsZ := ECItoECEF(j2000X, j2000Y, j2000Z, utc)
|
||||
// Convert ITRS to geodetic coordinates (WGS84)
|
||||
latitude, longitude, height := ECEFToGeodetic(itrsX, itrsY, itrsZ)
|
||||
latitude, longitude, height := ECEFGeocentricToGeodetic(itrsX, itrsY, itrsZ)
|
||||
return latitude, longitude, height
|
||||
}
|
||||
|
||||
func ECItoECEF(j2000X, j2000Y, j2000Z float64, utc time.Time) (float64, float64, float64) {
|
||||
func _ECItoECEF(j2000X, j2000Y, j2000Z float64, utc time.Time) (float64, float64, float64) {
|
||||
julianDate := UTCToJulianDate(utc)
|
||||
gast := CalculateGAST(julianDate, utc)
|
||||
|
||||
|
||||
54
pkg/calculator/sofa.go
Normal file
54
pkg/calculator/sofa.go
Normal file
@@ -0,0 +1,54 @@
|
||||
package calculator
|
||||
|
||||
import (
|
||||
"math"
|
||||
"time"
|
||||
|
||||
"github.com/hebl/gofa"
|
||||
)
|
||||
|
||||
func ECItoECEF(j2000X, j2000Y, j2000Z float64, utc time.Time) (float64, float64, float64) {
|
||||
j2000Position := [3]float64{j2000X, j2000Y, j2000Z}
|
||||
var ecef [3]float64
|
||||
|
||||
// 当前的 TT 时间 (Terrestrial Time)
|
||||
var tt1, tt2 float64
|
||||
gofa.Dtf2d("UTC",
|
||||
utc.Year(),
|
||||
int(utc.Month()),
|
||||
utc.Day(),
|
||||
utc.Hour(),
|
||||
utc.Minute(),
|
||||
float64(utc.Second()),
|
||||
&tt1, &tt2)
|
||||
|
||||
var djm0, mjd float64
|
||||
gofa.Cal2jd(int(utc.Year()), int(utc.Month()), int(utc.Day()), &djm0, &mjd)
|
||||
var dut1 float64 = 0.0562
|
||||
xp := 0.0
|
||||
yp := 0.0
|
||||
|
||||
if e, ok := EOP.Get(mjd); ok {
|
||||
xp = e.Xp
|
||||
yp = e.Yp
|
||||
dut1 = e.Dut1
|
||||
}
|
||||
|
||||
// 计算 UT1 时间 世界时UT1与协调世界时UTC
|
||||
var ut11, ut12 float64
|
||||
gofa.Utcut1(tt1, tt2, dut1, &ut11, &ut12)
|
||||
|
||||
// 计算 C2T 矩阵 (ICRS -> CIRS -> ECEF)
|
||||
var rc2t [3][3]float64
|
||||
gofa.C2t06a(tt1, tt2, ut11, ut12, xp, yp, &rc2t)
|
||||
|
||||
// 计算 ECEF 坐标
|
||||
gofa.Rxp(rc2t, j2000Position, &ecef)
|
||||
return ecef[0], ecef[1], ecef[2]
|
||||
}
|
||||
|
||||
func ECEFGeocentricToGeodetic(x, y, z float64) (float64, float64, float64) {
|
||||
var lat, lon, alt float64
|
||||
gofa.Gc2gd(gofa.WGS84, [3]float64{x, y, z}, &lon, &lat, &alt)
|
||||
return lat * 180 / math.Pi, lon * 180 / math.Pi, alt
|
||||
}
|
||||
Reference in New Issue
Block a user