辅助数据拟合

pkg/auxilary/gps.go

@@ -0,0 +1,126 @@
+package auxilary
+
+import (
+	"fmt"
+	"math"
+	"os"
+
+	"starwiz.cn/sjy01/image-proc/pkg/utils"
+)
+
+type GPSs struct {
+	GPSs []*GPS
+}
+
+func (g GPSs) Save(gpsFile string) error {
+	f, err := os.Create(gpsFile)
+	if err != nil {
+		return err
+	}
+	defer f.Close()
+
+	for _, gps := range g.GPSs {
+		content := fmt.Sprintf("%.8f %.8f %.8f %.8f %.8f %.8f %.8f\n",
+			gps.UTCTimestampSec,
+			gps.X84, gps.Y84, gps.Z84,
+			gps.Vx84, gps.Vy84, gps.Vz84)
+		f.WriteString(content)
+	}
+
+	return nil
+}
+
+// 采用拉格朗日内插方法获得gps位置
+// 输入：t为UTC时间戳，单位为秒
+func (g GPSs) Lagrange(t float64) *GPS {
+	// 找到扫描行成像时刻前后四个时间点的位置矢量
+	var gps_sample []*GPS
+	var idx int
+
+	if t < g.GPSs[0].UTCTimestampSec {
+		return g.GPSs[0]
+	} else if t > g.GPSs[len(g.GPSs)-1].UTCTimestampSec {
+		return g.GPSs[len(g.GPSs)-1]
+	}
+
+	for i, g := range g.GPSs {
+		if g.UTCTimestampSec <= t {
+			idx = i
+		}
+	}
+
+	idx0 := int(math.Max(0, float64(idx-3)))
+	for i := idx0; i <= idx; i++ {
+		gps_sample = append(gps_sample, g.GPSs[i])
+	}
+
+	idx1 := int(math.Min(float64(len(g.GPSs))-1, float64(idx+1)))
+	idx2 := int(math.Min(float64(len(g.GPSs))-1, float64(idx+4)))
+	for i := idx1; i <= idx2; i++ {
+		gps_sample = append(gps_sample, g.GPSs[i])
+	}
+
+	var x, y, z, tt []float64
+	for i := 0; i < len(gps_sample); i++ {
+		tt = append(tt, gps_sample[i].UTCTimestampSec)
+		x = append(x, gps_sample[i].X84)
+		y = append(y, gps_sample[i].Y84)
+		z = append(z, gps_sample[i].Z84)
+	}
+
+	px := utils.InterpLagrange(tt, x, t)
+	py := utils.InterpLagrange(tt, y, t)
+	pz := utils.InterpLagrange(tt, z, t)
+
+	return &GPS{
+		UTCTimestampSec: t,
+		X84:             px,
+		Y84:             py,
+		Z84:             pz,
+		Vx84:            g.GPSs[idx].Vx84,
+		Vy84:            g.GPSs[idx].Vy84,
+		Vz84:            g.GPSs[idx].Vz84,
+	}
+}
+
+type GPS struct {
+	UTCTimestampSec  float64
+	X84, Y84, Z84    float64
+	Vx84, Vy84, Vz84 float64
+}
+
+func StoreGPS(aps []*AuxPlatform, gpsFile string) (*GPSs, error) {
+	gpss := ExtractGPS(aps)
+	var interGPS GPSs
+	for _, gps := range gpss.GPSs {
+		interGPS.GPSs = append(interGPS.GPSs, gps)
+		t := gps.UTCTimestampSec + 0.098678123
+		interGPS.GPSs = append(interGPS.GPSs, gpss.Lagrange(t))
+	}
+
+	return gpss, interGPS.Save(gpsFile)
+}
+
+func ExtractGPS(aps []*AuxPlatform) *GPSs {
+	var gpss GPSs
+	var sec, microsec uint32
+	for _, ap := range aps {
+		if ap.UTCTimeSec != sec || ap.Microsecond != microsec {
+			sec, microsec = ap.UTCTimeSec, ap.Microsecond
+			gps := GPS{
+				UTCTimestampSec: float64(sec) + float64(ReferenceTime2000) +
+					float64(transfromGPSandAttMicrosec(microsec))/1e6,
+				X84:  ap.W84PosX,
+				Y84:  ap.W84PosY,
+				Z84:  ap.W84PosZ,
+				Vx84: ap.W84VelX,
+				Vy84: ap.W84VelY,
+				Vz84: ap.W84VelZ,
+			}
+
+			gpss.GPSs = append(gpss.GPSs, &gps)
+		}
+	}
+
+	return &gpss
+}