j2000-w84

calculator/const.go

@@ -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
+)

calculator/j2000.go

@@ -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},
+	}
+}

calculator/proj.go

@@ -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}
-}

calculator/wgs84.go

@@ -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
+}

cmd/parse.go

@@ -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")
 	},
 }
 

extract/aux_platform.go

@@ -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
+	Eular1         float64   // [49-52]本体相对轨道姿态角，[Z]Yaw 测摆角，量纲：1/1000000 单位：rad
-	Eular2         float64   // [53-56]
+	Eular2         float64   // [53-56] Pitch [Y]
-	Eular3         float64   // [57-60]
+	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.WGS84XYZtoLatLngH(ap.W84PosX, ap.W84PosY, ap.W84PosZ))
-		// pp.Println(calculator.WGS84XYZtoLngLat(ap.WGS84PosX, ap.WGS84PosY, ap.WGS84PosZ))
+		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
 }