从位置和速度计算验证轨道到地心坐标系的旋转矩阵

pkg/calculator/intersection.go

@@ -1,9 +1,11 @@
 package calculator
 
 import (
+	"fmt"
 	"math"
 	"time"
 
+	"github.com/sirupsen/logrus"
 	"gonum.org/v1/gonum/mat"
 )
 
@@ -13,7 +15,7 @@ type IntersectionPoint struct {
 	H   float64
 }
 
-func Intersection(q Quaternion, satPos84 []float64, satTime time.Time, ucam int) IntersectionPoint {
+func IntersectionAttitude(q Quaternion, satPos84 []float64, satTime time.Time, ucam int) (IntersectionPoint, error) {
 	// alpha := FOV * math.Pi / 180.0
 	// alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels))
 	// direction := []float64{0, math.Tan(alpha), -1.3}
@@ -35,12 +37,16 @@ func Intersection(q Quaternion, satPos84 []float64, satTime time.Time, ucam int)
 	dECEF := []float64{x, y, z}
 
 	// -------- 计算与地球表面的交点 --------
-	intersection := intersectWithEllipsoid(satPos84, dECEF)
+	intersection, err := intersectWithEllipsoid(satPos84, dECEF)
+	if err != nil {
+		return IntersectionPoint{}, err
+	}
+
 	lat, lon, h := ECEFToGeodetic(intersection[0], intersection[1], intersection[2])
-	return IntersectionPoint{Lat: lat, Lon: lon, H: h}
+	return IntersectionPoint{Lat: lat, Lon: lon, H: h}, nil
 }
 
-func Intersection2(Qsat2orbit, Qorbit2eci Quaternion, satPos84 []float64, satTime time.Time, ucam int) IntersectionPoint {
+func IntersectionECI(Qsat2orbit, Qorbit2eci Quaternion, satPos84 []float64, satTime time.Time, ucam int) (IntersectionPoint, error) {
 	alpha := FOV * math.Pi / 180.0
 	alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels))
 	direction := []float64{0, math.Tan(alpha), -1.3} // 卫星（相机）坐标系下CCD成像方向向量
@@ -67,14 +73,18 @@ func Intersection2(Qsat2orbit, Qorbit2eci Quaternion, satPos84 []float64, satTim
 	dECEF := []float64{x, y, z}
 
 	// -------- 计算交点 --------}
-	intersection := intersectWithEllipsoid(satPos84, dECEF)
+	intersection, err := intersectWithEllipsoid(satPos84, dECEF)
+	if err != nil {
+		return IntersectionPoint{}, err
+	}
+
 	lat, lon, h := ECEFToGeodetic(intersection[0], intersection[1], intersection[2])
 
-	return IntersectionPoint{Lat: lat, Lon: lon, H: h}
+	return IntersectionPoint{Lat: lat, Lon: lon, H: h}, nil
 }
 
 // 计算与椭球表面的交点
-func intersectWithEllipsoid(p0, d []float64) []float64 {
+func intersectWithEllipsoid(p0, d []float64) ([]float64, error) {
 	a2 := a * a
 	b2 := b * b
 
@@ -84,7 +94,8 @@ func intersectWithEllipsoid(p0, d []float64) []float64 {
 
 	delta := B*B - 4*A*C
 	if delta < 0 {
-		return nil // No intersection
+		logrus.Error("line of sight: no intersection with ellipsoid")
+		return nil, fmt.Errorf("line of sight: no intersection with ellipsoid") // No intersection
 	}
 	t1 := (-B + math.Sqrt(delta)) / (2 * A)
 	t2 := (-B - math.Sqrt(delta)) / (2 * A)
@@ -93,5 +104,5 @@ func intersectWithEllipsoid(p0, d []float64) []float64 {
 		p0[0] + t*d[0],
 		p0[1] + t*d[1],
 		p0[2] + t*d[2],
-	}
+	}, nil
 }