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

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
 }

pkg/calculator/orbit.go

@@ -0,0 +1,65 @@
+package calculator
+
+import (
+	"math"
+
+	"gonum.org/v1/gonum/mat"
+	"gonum.org/v1/gonum/spatial/r3"
+)
+
+// OrbitToECMatrix 轨道坐标系到ECI、ECEF坐标系的变换矩阵
+func OrbitToECMatrix(pos, vec []float64) *mat.Dense {
+	r := r3.Vec{X: pos[0], Y: pos[1], Z: pos[2]}
+	rmag := r3.Norm(r) // Magnitude
+	v := r3.Vec{X: vec[0], Y: vec[1], Z: vec[2]}
+	vmag := r3.Norm(v) // Magnitude of velocity vector
+	w := r3.Cross(v, r)
+	wmag := r3.Norm(w)
+
+	z0 := r3.Scale(-1/rmag, r) // z方向指向地心
+	y0 := r3.Scale(1/wmag, w)
+	x0 := r3.Scale(1/vmag, v)
+
+	m := mat.NewDense(3, 3, []float64{
+		x0.X, y0.X, z0.X,
+		x0.Y, y0.Y, z0.Y,
+		x0.Z, y0.Z, z0.Z,
+	})
+
+	return m
+}
+
+// IntersectionECEF 计算卫星与相机的交点，返回经纬度和高度
+// FIXME: 该计算方法有误，待修正
+func IntersectionECEF(Qsat2orbit Quaternion, satPos84, vec84 []float64, 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成像方向向量
+
+	// -------- 相机坐标系下CCD成像方向向量转到卫星坐标系 --------
+	Rcam := CameraRotMatrix(AngleCamSatX*math.Pi/180.0, AngleCamSatY*math.Pi/180.0, 0)
+	var dCam mat.VecDense
+	dCam.MulVec(Rcam, mat.NewVecDense(3, direction))
+
+	// -------- 转到轨道坐标系 --------
+	Rsat2orbit := Qsat2orbit.ToRotationMatrix()
+	var r0 mat.VecDense
+	r0.MulVec(Rsat2orbit, &dCam)
+	dOrbit := r0.RawVector().Data
+
+	// -------- 转到ECEF坐标系 --------
+	Rorbit2ecef := OrbitToECMatrix(satPos84, vec84)
+	var r1 mat.VecDense
+	r1.MulVec(Rorbit2ecef, mat.NewVecDense(3, dOrbit))
+	dECEF := r1.RawVector().Data
+
+	// -------- 计算交点 --------}
+	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}, err
+}