使用定姿四元数计算图像位置

pkg/calculator/const.go

@@ -5,6 +5,7 @@ const (
 	EarthRadius = 6378137.0         // 地球半径，单位米
 	a           = 6378137.0         // semi-major axis in meters
 	f           = 1 / 298.257223563 // flattening
+	b           = a * (1 - f)       // 短半轴
 	e2          = 2*f - f*f         // square of eccentricity
 	J2000Epoch  = 2451545.0         // Julian date of J2000 epoch
 )

pkg/calculator/intersection.go

@@ -0,0 +1,64 @@
+package calculator
+
+import (
+	"math"
+	"time"
+
+	"gonum.org/v1/gonum/mat"
+)
+
+// 常量
+const (
+	focal   = 1.3  // 焦距, m
+	FOV     = 1.7  // 视场角,degree
+	nPixels = 9344 // 像素数
+)
+
+func Intersection(q Quaternion, satPos []float64, satTime time.Time, ucam int) (float64, float64) {
+	alpha := FOV * math.Pi / 180.0
+	alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(nPixels))
+	direction := []float64{0, math.Tan(alpha), -1.3}
+
+	Ratt := q.ToRotationMatrix()
+	RattT := &mat.Dense{}
+	RattT.Inverse(Ratt)
+
+	v := mat.NewVecDense(3, direction)
+	var result mat.VecDense
+	result.MulVec(Ratt, v)
+	eciDirection := result.RawVector().Data
+
+	// intersection := intersectWithEllipsoid(satPos, eciDirection)
+	// lat, lon, _ := J2000ToWGS84(intersection[0], intersection[1], intersection[2], satTime)
+
+	x, y, z := ECItoECEF(eciDirection[0], eciDirection[1], eciDirection[2], satTime)
+	ecefDirection := []float64{x, y, z}
+	intersection := intersectWithEllipsoid(satPos, ecefDirection)
+	lat, lon, _ := ECEFToGeodetic(intersection[0], intersection[1], intersection[2])
+
+	return lat, lon
+
+}
+
+// 计算与椭球表面的交点
+func intersectWithEllipsoid(p0, d []float64) []float64 {
+	a2 := a * a
+	b2 := b * b
+
+	A := d[0]*d[0]/a2 + d[1]*d[1]/a2 + d[2]*d[2]/b2
+	B := 2 * (p0[0]*d[0]/a2 + p0[1]*d[1]/a2 + p0[2]*d[2]/b2)
+	C := p0[0]*p0[0]/a2 + p0[1]*p0[1]/a2 + p0[2]*p0[2]/b2 - 1
+
+	delta := B*B - 4*A*C
+	if delta < 0 {
+		return nil // No intersection
+	}
+	t1 := (-B + math.Sqrt(delta)) / (2 * A)
+	t2 := (-B - math.Sqrt(delta)) / (2 * A)
+	t := math.Max(t1, t2)
+	return []float64{
+		p0[0] + t*d[0],
+		p0[1] + t*d[1],
+		p0[2] + t*d[2],
+	}
+}

pkg/calculator/j2000.go

@@ -7,6 +7,13 @@ import (
 
 // Function to convert current J2000 position to WGS84
 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)
+	return latitude, longitude, height
+}
+
+func ECItoECEF(j2000X, j2000Y, j2000Z float64, utc time.Time) (float64, float64, float64) {
 	julianDate := UTCToJulianDate(utc)
 	gast := CalculateGAST(julianDate, utc)
 
@@ -18,9 +25,7 @@ func J2000ToWGS84(j2000X, j2000Y, j2000Z float64, utc time.Time) (float64, float
 	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
+	return itrsX, itrsY, itrsZ
 }
 
 // Function to convert UTC to Julian Date

pkg/calculator/quaternion.go

@@ -1,70 +1,20 @@
 package calculator
 
 import (
-	"fmt"
-	"math"
+	"gonum.org/v1/gonum/mat"
 )
 
-// Quaternion represents a quaternion with scalar (w) and vector (x, y, z) parts
 type Quaternion struct {
-	w, x, y, z float64
+	W, X, Y, Z float64
 }
 
-// Quaternion multiplication
-func (q1 Quaternion) Mul(q2 Quaternion) Quaternion {
-	return Quaternion{
-		w: q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z,
-		x: q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y,
-		y: q1.w*q2.y - q1.x*q2.z + q1.y*q2.w + q1.z*q2.x,
-		z: q1.w*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.w,
-	}
-}
-
-// Quaternion conjugate
-func (q Quaternion) Conjugate() Quaternion {
-	return Quaternion{w: q.w, x: -q.x, y: -q.y, z: -q.z}
-}
-
-// Rotate vector by quaternion
-func (q Quaternion) Rotate(v [3]float64) [3]float64 {
-	qv := Quaternion{w: 0, x: v[0], y: v[1], z: v[2]}
-	qConj := q.Conjugate()
-	qvRotated := q.Mul(qv).Mul(qConj)
-	return [3]float64{qvRotated.x, qvRotated.y, qvRotated.z}
-}
-
-func main() {
-	// 示例数据
-	qBI := Quaternion{w: 1, x: 0, y: 0, z: 0} // 本体相对惯性系四元数
-	posJ2000 := [3]float64{7000, 0, 0}        // J2000位置
-	// velJ2000 := [3]float64{0, 7.5, 0}         // J2000速度
-
-	// 相机参数
-	const numPixels = 9520
-	const fov = 10.0 * math.Pi / 180 // 假设视场角为10度
-
-	// 逐像素计算地面交点
-	for i := 0; i < numPixels; i++ {
-		// 计算像素点相对光轴的偏角
-		pixelOffset := (float64(i) - float64(numPixels)/2) / float64(numPixels)
-		angle := pixelOffset * fov
-
-		// 假设光轴在本体坐标系中指向-z方向，计算视线方向
-		dBody := [3]float64{-math.Sin(angle), 0, -math.Cos(angle)}
-
-		// 转换到惯性系
-		dInertial := qBI.Rotate(dBody)
-
-		// 计算地面交点（假设dInertial已经标准化）
-		k := -posJ2000[2] / dInertial[2] // 简化的交点计算
-		groundPoint := [3]float64{
-			posJ2000[0] + k*dInertial[0],
-			posJ2000[1] + k*dInertial[1],
-			posJ2000[2] + k*dInertial[2],
-		}
-
-		// 转换到地理坐标
-		lat, lon, _ := ECEFToGeodetic(groundPoint[0], groundPoint[1], groundPoint[2])
-		fmt.Printf("Pixel %d: Latitude: %f, Longitude: %f\n", i, lat, lon)
-	}
+// ToRotationMatrix converts a quaternion to a rotation matrix.
+func (q Quaternion) ToRotationMatrix() *mat.Dense {
+	w, x, y, z := q.W, q.X, q.Y, q.Z
+
+	return mat.NewDense(3, 3, []float64{
+		1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y,
+		2*x*y + 2*w*z, 1 - 2*x*x - 2*z*z, 2*y*z - 2*w*x,
+		2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y,
+	})
 }