115 lines
3.7 KiB
Go
115 lines
3.7 KiB
Go
package extract
|
||
|
||
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, satelliteVel Vector3, quat Quaternion) {
|
||
// 假设一些已知数据
|
||
// 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)
|
||
}
|