get raw data

aux/attitude.go

@@ -0,0 +1,10 @@
+package aux
+
+/*
+   将欧拉角 (yaw, pitch, roll) 转换为旋转矩阵
+
+   参数:
+   - yaw: 偏航角（绕Z轴旋转）
+   - pitch: 俯仰角（绕Y轴旋转）
+   - roll: 滚转角（绕X轴旋转）
+*/

aux/aux.go

@@ -0,0 +1,114 @@
+package aux
+
+import (
+	"fmt"
+	"math"
+)
+
+// 定义常数
+const (
+	GM = 3.986004418e14 // 地球引力常数（m^3/s^2）
+	Re = 6378137.0      // 地球半径（m）
+)
+
+// 四元数
+type Quaternion struct {
+	w, x, y, z float64
+}
+
+// 矢量
+type Vector3 struct {
+	x, y, z float64
+}
+
+// 计算轨道参数
+func calculateOrbitalParameters(position, velocity Vector3) (float64, float64, float64, float64, float64) {
+	r := math.Sqrt(position.x*position.x + position.y*position.y + position.z*position.z)
+	v := math.Sqrt(velocity.x*velocity.x + velocity.y*velocity.y + velocity.z*velocity.z)
+
+	// 计算轨道参数
+	energy := 0.5*v*v - GM/r // 比能
+	a := -GM / (2 * energy)  // 轨道半长轴
+	e := math.Sqrt(1 - (math.Pow(math.Sqrt(math.Pow(position.x*velocity.y-position.y*velocity.x, 2)+
+		math.Pow(position.y*velocity.z-position.z*velocity.y, 2)+
+		math.Pow(position.z*velocity.x-position.x*velocity.z, 2)), 2) / (GM * a)))
+	i := math.Acos(position.z / r)                                                                                // 轨道倾角
+	Omega := math.Atan2(position.x, -position.y)                                                                  // 升交点赤经
+	omega := math.Atan2(position.z*velocity.x-position.x*velocity.z, position.x*velocity.y-position.y*velocity.x) // 升交点赤纬
+	return a, e, i * 180 / math.Pi, Omega * 180 / math.Pi, omega * 180 / math.Pi
+}
+
+// 四元数到旋转矩阵
+func quaternionToRotationMatrix(q Quaternion) [3][3]float64 {
+	w, x, y, z := q.w, q.x, q.y, q.z
+	return [3][3]float64{
+		{1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w},
+		{2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w},
+		{2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y},
+	}
+}
+
+// 向量加法
+func add(v1, v2 Vector3) Vector3 {
+	return Vector3{v1.x + v2.x, v1.y + v2.y, v1.z + v2.z}
+}
+
+// 向量数乘
+func scale(v Vector3, c float64) Vector3 {
+	return Vector3{v.x * c, v.y * c, v.z * c}
+}
+
+// 矩阵乘法
+func matrixMult(m [3][3]float64, v Vector3) Vector3 {
+	return Vector3{
+		m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z,
+		m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z,
+		m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z,
+	}
+}
+
+// 计算图像位置
+func calculateImagePosition(roll, pitch, yaw float64, quat Quaternion, satellitePos, satelliteVel Vector3) (float64, float64) {
+	// 将角度转换为弧度
+	roll = roll * math.Pi / 180
+	pitch = pitch * math.Pi / 180
+	yaw = yaw * math.Pi / 180
+
+	// 构造旋转矩阵
+	R := quaternionToRotationMatrix(quat)
+	// 传感器坐标系中的Z轴向量
+	sensorZ := Vector3{0, 0, 1}
+	// 将Z轴向量通过旋转矩阵转换到卫星坐标系中
+	lineOfSight := matrixMult(R, sensorZ)
+
+	// 计算交点经纬度
+	lat := math.Asin(lineOfSight.z) * 180 / math.Pi
+	lon := math.Atan2(lineOfSight.y, lineOfSight.x) * 180 / math.Pi
+
+	return lat, lon
+}
+
+func Calculate() {
+	// 假设一些已知数据
+	satellitePos := Vector3{x: 7000e3, y: 0, z: 0}       // 假设一个简单的地心坐标位置
+	satelliteVel := Vector3{x: 0, y: 7.5e3, z: 0}        // 假设一个简单的速度
+	quat := Quaternion{w: 0.7071, x: 0.7071, y: 0, z: 0} // 假设一个简单的四元数
+	roll := 0.0
+	pitch := 0.0
+	yaw := 0.0
+
+	// 计算轨道参数
+	a, e, i, Omega, omega := calculateOrbitalParameters(satellitePos, satelliteVel)
+
+	// 打印轨道参数
+	fmt.Println("轨道参数：")
+	fmt.Printf("轨道半长轴 (a): %.2f m\n", a)
+	fmt.Printf("偏心率 (e): %.6f\n", e)
+	fmt.Printf("轨道倾角 (i): %.2f 度\n", i)
+	fmt.Printf("升交点赤经 (Ω): %.2f 度\n", Omega)
+	fmt.Printf("近地点幅角 (ω): %.2f 度\n", omega)
+
+	// 计算图像位置
+	lat, lon := calculateImagePosition(roll, pitch, yaw, quat, satellitePos, satelliteVel)
+	fmt.Printf("\n图像位置：\n纬度: %.6f\n经度: %.6f\n", lat, lon)
+}

aux/orbit.go

@@ -0,0 +1 @@
+package aux