127 lines
2.8 KiB
Go
127 lines
2.8 KiB
Go
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("%.6f %.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 < 1e-7 {
|
||
return g.GPSs[0]
|
||
} else if t-g.GPSs[len(g.GPSs)-1].UTCTimestampSec > -1e-7 {
|
||
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, gpss.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
|
||
}
|