j2000-w84
This commit is contained in:
nuknal
9
calculator/const.go
Normal file
9
calculator/const.go
Normal file
@@ -0,0 +1,9 @@
|
||||
package calculator
|
||||
|
||||
// WGS84 ellipsoid constants
|
||||
const (
|
||||
a = 6378137.0 // semi-major axis in meters
|
||||
f = 1 / 298.257223563 // flattening
|
||||
e2 = 2*f - f*f // square of eccentricity
|
||||
J2000Epoch = 2451545.0 // Julian date of J2000 epoch
|
||||
)
|
||||
78
calculator/j2000.go
Normal file
78
calculator/j2000.go
Normal file
@@ -0,0 +1,78 @@
|
||||
package calculator
|
||||
|
||||
import (
|
||||
"math"
|
||||
"time"
|
||||
)
|
||||
|
||||
// Function to convert current J2000 position to WGS84
|
||||
func J2000ToWGS84(j2000X, j2000Y, j2000Z float64, utc time.Time) (float64, float64, float64) {
|
||||
julianDate := UTCToJulianDate(utc)
|
||||
gast := CalculateGAST(julianDate, utc)
|
||||
|
||||
// Calculate rotation matrix from J2000 to ITRS
|
||||
rotationMatrix := CreateRotationMatrix(gast)
|
||||
|
||||
// Rotate J2000 position to ITRS
|
||||
itrsX := rotationMatrix[0][0]*j2000X + rotationMatrix[0][1]*j2000Y + rotationMatrix[0][2]*j2000Z
|
||||
itrsY := rotationMatrix[1][0]*j2000X + rotationMatrix[1][1]*j2000Y + rotationMatrix[1][2]*j2000Z
|
||||
itrsZ := rotationMatrix[2][0]*j2000X + rotationMatrix[2][1]*j2000Y + rotationMatrix[2][2]*j2000Z
|
||||
|
||||
// Convert ITRS to geodetic coordinates (WGS84)
|
||||
latitude, longitude, height := ECEFToGeodetic(itrsX, itrsY, itrsZ)
|
||||
return latitude, longitude, height
|
||||
}
|
||||
|
||||
// Function to convert UTC to Julian Date
|
||||
func UTCToJulianDate(t time.Time) float64 {
|
||||
year, month, day := t.Date()
|
||||
hour, minute, second := t.Clock()
|
||||
fracOfDay := float64(hour)/24 + float64(minute)/(24*60) + float64(second)/(24*60*60)
|
||||
|
||||
if month <= 2 {
|
||||
year -= 1
|
||||
month += 12
|
||||
}
|
||||
|
||||
A := year / 100
|
||||
B := 2 - A + A/4
|
||||
julianDate := float64(int(365.25*float64(year))) + float64(int(30.6001*float64(month+1))) + float64(day) + 1720994.5 + fracOfDay + float64(B)
|
||||
return julianDate
|
||||
}
|
||||
|
||||
// Function to calculate GAST (Greenwich Apparent Sidereal Time)
|
||||
func CalculateGAST(julianDate float64, utc time.Time) float64 {
|
||||
// Calculate Julian centuries since J2000.0
|
||||
T := (julianDate - J2000Epoch) / 36525.0
|
||||
|
||||
// Calculate Greenwich Mean Sidereal Time (GMST) in degrees
|
||||
GMST := 280.46061837 + 360.98564736629*(julianDate-J2000Epoch) + 0.000387933*T*T - T*T*T/38710000
|
||||
GMST = math.Mod(GMST, 360.0)
|
||||
if GMST < 0 {
|
||||
GMST += 360.0
|
||||
}
|
||||
|
||||
// Calculate the equation of the equinoxes (simplified)
|
||||
omega := 125.04 - 0.052954*T
|
||||
L := 280.47 + 0.98565*T
|
||||
epsilon := 23.4393 - 0.0000004*T
|
||||
deltaPsi := -0.000319*math.Sin(math.Pi/180.0*omega) - 0.000024*math.Sin(math.Pi/180.0*2*L)
|
||||
|
||||
// Greenwich Apparent Sidereal Time (GAST)
|
||||
GAST := GMST + deltaPsi*math.Cos(math.Pi/180.0*epsilon)
|
||||
GAST = math.Mod(GAST, 360.0)
|
||||
if GAST < 0 {
|
||||
GAST += 360.0
|
||||
}
|
||||
|
||||
return GAST * math.Pi / 180.0 // Convert to radians
|
||||
}
|
||||
|
||||
// Function to create the rotation matrix for the given GAST
|
||||
func CreateRotationMatrix(gast float64) [3][3]float64 {
|
||||
return [3][3]float64{
|
||||
{math.Cos(gast), math.Sin(gast), 0},
|
||||
{-math.Sin(gast), math.Cos(gast), 0},
|
||||
{0, 0, 1},
|
||||
}
|
||||
}
|
||||
@@ -1,24 +0,0 @@
|
||||
package calculator
|
||||
|
||||
import "github.com/twpayne/go-proj/v10"
|
||||
|
||||
func J2000ToLngLat(x, y, z float64) []float64 {
|
||||
var lng, lat float64
|
||||
return []float64{lng, lat}
|
||||
}
|
||||
|
||||
func WGS84XYZtoLngLat(x, y, z float64) []float64 {
|
||||
var lng, lat float64
|
||||
pj, err := proj.NewCRSToCRS("EPSG:3857", "EPSG:4326", nil)
|
||||
if err != nil {
|
||||
panic(err)
|
||||
}
|
||||
coor := proj.NewCoord(x, y, z, 0)
|
||||
coor, err = pj.Forward(coor)
|
||||
if err != nil {
|
||||
panic(err)
|
||||
}
|
||||
lng = coor.X()
|
||||
lat = coor.Y()
|
||||
return []float64{lng, lat}
|
||||
}
|
||||
31
calculator/wgs84.go
Normal file
31
calculator/wgs84.go
Normal file
@@ -0,0 +1,31 @@
|
||||
package calculator
|
||||
|
||||
import "math"
|
||||
|
||||
func WGS84XYZtoLatLngH(X, Y, Z float64) (float64, float64, float64) {
|
||||
return ECEFToGeodetic(X, Y, Z)
|
||||
}
|
||||
|
||||
// Function to convert ECEF (ITRS) coordinates to geodetic coordinates (latitude, longitude, height)
|
||||
func ECEFToGeodetic(X, Y, Z float64) (float64, float64, float64) {
|
||||
b := a * (1 - f)
|
||||
e2Prime := e2 * (a * a) / (b * b)
|
||||
p := math.Sqrt(X*X + Y*Y)
|
||||
theta := math.Atan2(Z*a, p*b)
|
||||
|
||||
// Calculate Longitude
|
||||
longitude := math.Atan2(Y, X)
|
||||
|
||||
// Calculate Latitude
|
||||
latitude := math.Atan2(Z+e2Prime*b*math.Pow(math.Sin(theta), 3), p-e2*a*math.Pow(math.Cos(theta), 3))
|
||||
|
||||
// Calculate Height
|
||||
N := a / math.Sqrt(1-e2*math.Pow(math.Sin(latitude), 2))
|
||||
height := p/math.Cos(latitude) - N
|
||||
|
||||
// Convert radians to degrees for latitude and longitude
|
||||
latitudeDeg := latitude * 180 / math.Pi
|
||||
longitudeDeg := longitude * 180 / math.Pi
|
||||
|
||||
return latitudeDeg, longitudeDeg, height
|
||||
}
|
||||
@@ -29,6 +29,7 @@ var parseCmd = &cobra.Command{
|
||||
logrus.Error(err)
|
||||
}
|
||||
// p.ParseAuxEBox("demo/output/SJY01_PMS_20240516_101236_051622_096_EB.AUX")
|
||||
e.ParseAuxPlatform("demo/output/Q052100/SJY01_PMS_20240519_121433_Q052100_102.AUX")
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
@@ -9,6 +9,7 @@ import (
|
||||
"github.com/k0kubun/pp/v3"
|
||||
"github.com/sirupsen/logrus"
|
||||
log "github.com/sirupsen/logrus"
|
||||
"starwiz.cn/sjy01/preprocessing/calculator"
|
||||
)
|
||||
|
||||
const AuxPlatformFrmSize = 512 // 512 bytes
|
||||
@@ -29,9 +30,9 @@ type AuxPlatform struct {
|
||||
QuatOrbJQ1 float64 // [37-40]轨道相对惯性系四元数矢部 的 Q1 值，量纲 1/0x40000000
|
||||
QuatOrbJQ2 float64 // [41-44]Q2 值
|
||||
QuatOrbJQ3 float64 // [45-48]Q3 值
|
||||
Eular1 float64 // [49-52]本体相对轨道姿态角，量纲：1/1000000 单位：rad
|
||||
Eular2 float64 // [53-56]
|
||||
Eular3 float64 // [57-60]
|
||||
Eular1 float64 // [49-52]本体相对轨道姿态角，[Z]Yaw 测摆角，量纲：1/1000000 单位：rad
|
||||
Eular2 float64 // [53-56] Pitch [Y]
|
||||
Eular3 float64 // [57-60] Roll [X]
|
||||
DotEular1 float64 // [61-62]本体相对轨道角速度，量纲：1/100000 单位：rad
|
||||
DotEular2 float64 // [63-64]
|
||||
DotEular3 float64 // [65-66]
|
||||
@@ -353,23 +354,32 @@ func (e *Extractor) ParseAuxPlatform(auxfile string) ([]*AuxPlatform, error) {
|
||||
}
|
||||
|
||||
aps = append(aps, &ap)
|
||||
ap.Print()
|
||||
// ap.Print()
|
||||
// Calculate(
|
||||
// Vector3{ap.J2000Pos_X, ap.J2000Pos_Y, ap.J2000Pos_Y},
|
||||
// Vector3{ap.J2000Vel_X, ap.J2000Vel_Y, ap.J2000Vel_Z},
|
||||
// Quaternion{ap.SS1_Q1, ap.SS1_Q2, ap.SS1_Q3, ap.SS1_Q4},
|
||||
// )
|
||||
|
||||
// pp.Println(calculator.WGS84XYZtoLngLat(ap.W84PosX, ap.W84PosY, ap.W84PosZ))
|
||||
// pp.Println(calculator.WGS84XYZtoLngLat(ap.WGS84PosX, ap.WGS84PosY, ap.WGS84PosZ))
|
||||
pp.Println(calculator.WGS84XYZtoLatLngH(ap.W84PosX, ap.W84PosY, ap.W84PosZ))
|
||||
pp.Println(calculator.WGS84XYZtoLatLngH(ap.WGS84PosX, ap.WGS84PosY, ap.WGS84PosZ))
|
||||
|
||||
pp.Println(calculator.J2000ToWGS84(
|
||||
ap.J2000PosX,
|
||||
ap.J2000PosY,
|
||||
ap.J2000PosZ,
|
||||
time.Unix(
|
||||
int64(ap.UTCTimeSec+uint32(ReferenceTime2000)),
|
||||
int64(ap.Microsecond)*1000).UTC()))
|
||||
|
||||
pp.Println(calculator.J2000ToWGS84(
|
||||
ap.J2000Pos_X,
|
||||
ap.J2000Pos_Y,
|
||||
ap.J2000Pos_Z,
|
||||
time.Unix(
|
||||
int64(ap.UTCTimeSec+uint32(ReferenceTime2000)),
|
||||
int64(ap.Microsecond)*1000).UTC()))
|
||||
break
|
||||
|
||||
if i+AuxPlatformFrmSize > len(data) {
|
||||
logrus.Info("rest of aux data length is not enough", len(data)-i)
|
||||
break
|
||||
}
|
||||
|
||||
}
|
||||
return aps, nil
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user