拟合GPS位置

pkg/utils/interp_lagrange.go

@@ -0,0 +1,120 @@
+package utils
+
+import (
+	"fmt"
+	"sort"
+
+	"github.com/chfenger/goNum"
+)
+
+type LagrangeInterpolator struct {
+	coeffs []float64
+	n      int
+}
+
+func (li *LagrangeInterpolator) Fit(x []float64, y []float64) error {
+	li.n = len(x) - 1
+	if li.n < 0 || len(y) != li.n+1 {
+		return fmt.Errorf("invalid input data")
+	}
+
+	if li.n > 9 {
+		li.n = 9 // 限制最大阶数为9
+	}
+
+	n := li.n + 1
+
+	// 初始化系数数组
+	li.coeffs = make([]float64, n)
+	for i := range li.coeffs {
+		li.coeffs[i] = 0
+	}
+
+	// 计算拉格朗日插值多项式的系数
+	for i := 0; i < n; i++ {
+		li_coeff := make([]float64, n)
+		li_coeff[0] = 1
+		for j := 0; j < n; j++ {
+			if i != j {
+				for k := n - 1; k >= 0; k-- {
+					li_coeff[k] *= -x[j]
+					if k > 0 {
+						li_coeff[k] += li_coeff[k-1]
+					}
+				}
+				for k := 0; k < n; k++ {
+					li_coeff[k] /= (x[i] - x[j])
+				}
+			}
+		}
+		for k := 0; k < n; k++ {
+			li.coeffs[k] += y[i] * li_coeff[k]
+		}
+	}
+
+	return nil
+}
+
+func (li LagrangeInterpolator) Predict(x float64) float64 {
+	n := len(li.coeffs)
+	y := 0.0
+	for i := 0; i < n; i++ {
+		term := li.coeffs[i]
+		for j := 0; j < i; j++ {
+			term *= x
+		}
+		y += term
+	}
+	return y
+}
+
+func (li LagrangeInterpolator) N() int {
+	return li.n
+}
+
+// InterpLagrange 利用拉格朗日插值法计算函数值
+// 尽量9阶采用内插值
+const STEP_N = 7
+
+func InterpLagrange(x []float64, y []float64, xq float64) float64 {
+	if len(x) != len(y) {
+		return 0.0
+	}
+
+	// 限制阶数为9
+	var data []float64
+	start, end := FindClosestSubset(x, xq, STEP_N)
+	for i := start; i <= end; i++ {
+		data = append(data, x[i])
+		data = append(data, y[i])
+	}
+
+	A := goNum.NewMatrix(len(data)/2, 2, data)
+	yq, _ := goNum.InterpLagrange(A, xq)
+	return yq
+}
+
+// FindClosestSubset 找到包含xq的最近的n个元素的子数组
+func FindClosestSubset(x []float64, xq float64, n int) (int, int) {
+	if len(x) <= n {
+		return 0, len(x) - 1 // 如果元素数量少于等于n，直接返回整个数组
+	}
+
+	// 找到xq在数组中的插入点
+	idx := sort.Search(len(x), func(i int) bool { return x[i] >= xq })
+
+	// 计算子数组的起始和结束位置
+	start := idx - n/2 // 尽量让xq在中间，4是因为9个元素的中间位置是4
+	end := idx + n/2
+
+	// 调整边界
+	if start < 0 {
+		start = 0
+		end = n
+	} else if end >= n {
+		end = n - 1
+		start = end - n + 1
+	}
+
+	return start, end
+}

pkg/utils/interp_lagrange_test.go

@@ -0,0 +1,29 @@
+package utils
+
+import (
+	"fmt"
+	"testing"
+)
+
+func TestInterpLagrange(t *testing.T) {
+	x := []float64{0, 1, 2, 3, 4}
+	y := []float64{0, 1, 4, 9, 16}
+
+	interp := &LagrangeInterpolator{}
+	if err := interp.Fit(x, y); err != nil {
+		t.Error(err)
+	}
+
+	fmt.Println("x = 2.5, y =", interp.Predict(2.5))
+	fmt.Println("x = 5.5, y =", interp.Predict(5.5))
+	fmt.Println("x = 2, y =", interp.Predict(2.0))
+
+	p := &PolynomialInterpolator{}
+	if err := p.Fit(x, y); err != nil {
+		t.Error(err)
+	}
+
+	fmt.Println("x = 2.5, y =", p.Predict(2.5))
+	fmt.Println("x = 5.5, y =", p.Predict(5.5))
+	fmt.Println("x = 2, y =", p.Predict(2.0))
+}

pkg/utils/interp_polynomial.go

@@ -0,0 +1,58 @@
+package utils
+
+import (
+	"math"
+
+	"gonum.org/v1/gonum/mat"
+)
+
+type PolynomialInterpolator struct {
+	Degree int
+	Coeffs []float64
+}
+
+func (p *PolynomialInterpolator) Fit(x, y []float64) error {
+	if p.Degree == 0 {
+		p.Degree = len(x) - 1
+	}
+
+	degree := p.Degree
+	n := len(x)
+	// Create the Vandermonde matrix
+	vander := mat.NewDense(n, degree+1, nil)
+	for i := 0; i < n; i++ {
+		for j := 0; j <= degree; j++ {
+			vander.Set(i, j, math.Pow(x[i], float64(j)))
+		}
+	}
+
+	// Create the right-hand side vector
+	yVec := mat.NewVecDense(n, y)
+
+	// Solve the least squares problem
+	var qr mat.QR
+	qr.Factorize(vander)
+	coeffs := mat.NewDense(degree+1, 1, nil)
+	err := qr.SolveTo(coeffs, false, yVec)
+	p.Coeffs = coeffs.RawMatrix().Data
+	return err
+}
+
+func (p PolynomialInterpolator) Predict(x float64) float64 {
+	var y float64
+	for i, coeff := range p.Coeffs {
+		y += coeff * math.Pow(x, float64(i))
+	}
+	return y
+}
+
+func InterpPolynomial(x []float64, y []float64, xq float64) float64 {
+	if len(x) != len(y) {
+		return 0.0
+	}
+
+	start, end := FindClosestSubset(x, xq, 4)
+	interp := &PolynomialInterpolator{Degree: 3}
+	interp.Fit(x[start:end+1], y[start:end+1])
+	return interp.Predict(xq)
+}