405 lines
9.3 KiB
Go
405 lines
9.3 KiB
Go
package producer
|
|
|
|
import (
|
|
"fmt"
|
|
"math"
|
|
"sort"
|
|
|
|
log "github.com/sirupsen/logrus"
|
|
"gonum.org/v1/gonum/mat"
|
|
)
|
|
|
|
type VX struct {
|
|
v float64
|
|
x mat.VecDense
|
|
}
|
|
|
|
type SolveMethod int
|
|
|
|
const (
|
|
SolveMethodOptimize SolveMethod = iota
|
|
SolveMethodLeastSquare
|
|
SolveMethodNelderMead
|
|
)
|
|
|
|
const epsilon = 1e-4
|
|
|
|
func normalize(v *mat.VecDense) (*mat.VecDense, float64, float64) {
|
|
var vOff, vScale float64
|
|
vOff = mat.Sum(v) / float64(v.Len())
|
|
vScale = math.Max(math.Abs(mat.Max(v)-vOff), math.Abs(mat.Min(v)-vOff))
|
|
for i := 0; i < v.Len(); i++ {
|
|
v.SetVec(i, (v.AtVec(i)-vOff)/vScale)
|
|
}
|
|
|
|
return v, vOff, vScale
|
|
}
|
|
|
|
func normalize2(v *mat.VecDense, vOff, vScale float64) *mat.VecDense {
|
|
for i := 0; i < v.Len(); i++ {
|
|
v.SetVec(i, (v.AtVec(i)-vOff)/vScale)
|
|
}
|
|
|
|
return v
|
|
}
|
|
|
|
func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense, method SolveMethod) ([]float64, error) {
|
|
M := setupSystemOfEquations(f, latVec, lonVec, heightVec)
|
|
n := f.Len()
|
|
|
|
// 计算初始值 x = (M^T * M)^-1 * (M^T * f)
|
|
var x0 mat.VecDense
|
|
var MtM mat.Dense
|
|
MtM.Mul(M.T(), M)
|
|
invMtM, err := invertRPCMatrix(&MtM)
|
|
if err != nil {
|
|
return nil, err
|
|
}
|
|
var MtF mat.VecDense
|
|
MtF.MulVec(M.T(), f)
|
|
x0.MulVec(invMtM, &MtF)
|
|
// return mat.Col(nil, 0, &x0), nil
|
|
|
|
if method == SolveMethodNelderMead {
|
|
numerator := mat.NewVecDense(20, nil)
|
|
denominator := mat.NewVecDense(20, nil)
|
|
denominator.SetVec(0, 1.0)
|
|
numerator.SetVec(0, x0.AtVec(0))
|
|
for i := 1; i < 20; i++ {
|
|
numerator.SetVec(i, x0.AtVec(i))
|
|
denominator.SetVec(i, x0.AtVec(i+19))
|
|
}
|
|
|
|
num, den, err := solveNelderMead(numerator, denominator, f, latVec, lonVec, heightVec)
|
|
if err != nil {
|
|
return nil, err
|
|
}
|
|
|
|
var coeffs []float64
|
|
coeffs = append(coeffs, num.RawVector().Data...)
|
|
for i := 1; i < 20; i++ {
|
|
coeffs = append(coeffs, den.AtVec(i))
|
|
}
|
|
return coeffs, nil
|
|
}
|
|
|
|
// 迭代
|
|
|
|
var x1 mat.VecDense
|
|
|
|
var vx []*VX
|
|
iterations := 0
|
|
maxIterations := 10
|
|
denominator := mat.NewVecDense(20, nil)
|
|
w2 := mat.NewDiagDense(n, nil)
|
|
var MtW2, MtW2M mat.Dense
|
|
var MtW2F mat.VecDense
|
|
for iterations < maxIterations {
|
|
denominator.SetVec(0, 1.0)
|
|
for idx := 1; idx < 20; idx++ {
|
|
denominator.SetVec(idx, x0.AtVec(idx+19))
|
|
}
|
|
weights := setupWeightMatrix(denominator, latVec, lonVec, heightVec)
|
|
for i := 0; i < n; i++ {
|
|
w2.SetDiag(i, weights.At(i, i)*weights.At(i, i))
|
|
}
|
|
MtW2.Mul(M.T(), w2)
|
|
MtW2M.Mul(&MtW2, M)
|
|
invMtW2M, err := invertRPCMatrix(&MtW2M)
|
|
if err != nil {
|
|
return nil, err
|
|
}
|
|
MtW2F.MulVec(&MtW2, f)
|
|
x1.MulVec(invMtW2M, &MtW2F)
|
|
|
|
numerator1 := mat.NewVecDense(20, nil)
|
|
denominator1 := mat.NewVecDense(20, nil)
|
|
denominator1.SetVec(0, 1.0)
|
|
numerator1.SetVec(0, x0.AtVec(0))
|
|
for i := 1; i < 20; i++ {
|
|
numerator1.SetVec(i, x1.AtVec(i))
|
|
denominator1.SetVec(i, x1.AtVec(i+19))
|
|
}
|
|
|
|
errorSquared := 0.0
|
|
lambda := 1e-4
|
|
for i := 0; i < n; i++ {
|
|
predictedV := project(numerator1, denominator1, latVec.AtVec(i), lonVec.AtVec(i), heightVec.AtVec(i))
|
|
errorV := predictedV - f.AtVec(i)
|
|
errorSquared += errorV * errorV
|
|
}
|
|
var coeffsSquared float64
|
|
for i := 0; i < 20; i++ {
|
|
coeffsSquared += lambda * (numerator1.AtVec(i)*numerator1.AtVec(i) + denominator1.AtVec(i)*denominator1.AtVec(i))
|
|
}
|
|
|
|
x0 = x1
|
|
iterations++
|
|
|
|
vx = append(vx, &VX{v: errorSquared, x: x1})
|
|
if errorSquared < 0.001 {
|
|
break
|
|
}
|
|
}
|
|
|
|
log.Println("iterations:", iterations)
|
|
if iterations == maxIterations {
|
|
sort.Slice(vx, func(i, j int) bool {
|
|
return vx[i].v < vx[j].v
|
|
})
|
|
x0 = vx[0].x
|
|
log.Printf("select min v-err: %.8f", vx[0].v)
|
|
}
|
|
|
|
return mat.Col(nil, 0, &x0), nil
|
|
}
|
|
|
|
func setupSystemOfEquations(Rn, latVec, lonVec, heightVec *mat.VecDense) *mat.Dense {
|
|
n := latVec.Len()
|
|
// 设计矩阵 B = [ 20个分子系数 19个分母系数 ]
|
|
B := mat.NewDense(n, 39, nil)
|
|
for i := 0; i < n; i++ {
|
|
P := latVec.AtVec(i)
|
|
L := lonVec.AtVec(i)
|
|
H := heightVec.AtVec(i)
|
|
r := Rn.AtVec(i)
|
|
|
|
B.Set(i, 0, 1)
|
|
B.Set(i, 1, L)
|
|
B.Set(i, 2, P)
|
|
B.Set(i, 3, H)
|
|
B.Set(i, 4, L*P)
|
|
B.Set(i, 5, L*H)
|
|
B.Set(i, 6, P*H)
|
|
B.Set(i, 7, L*L)
|
|
B.Set(i, 8, P*P)
|
|
B.Set(i, 9, H*H)
|
|
B.Set(i, 10, L*P*H)
|
|
B.Set(i, 11, L*L*L)
|
|
B.Set(i, 12, L*P*P)
|
|
B.Set(i, 13, L*H*H)
|
|
B.Set(i, 14, L*L*P)
|
|
B.Set(i, 15, P*P*P)
|
|
B.Set(i, 16, P*H*H)
|
|
B.Set(i, 17, L*L*H)
|
|
B.Set(i, 18, P*P*H)
|
|
B.Set(i, 19, H*H*H)
|
|
B.Set(i, 20, -L*r)
|
|
B.Set(i, 21, -P*r)
|
|
B.Set(i, 22, -H*r)
|
|
B.Set(i, 23, -L*P*r)
|
|
B.Set(i, 24, -L*H*r)
|
|
B.Set(i, 25, -P*H*r)
|
|
B.Set(i, 26, -L*L*r)
|
|
B.Set(i, 27, -P*P*r)
|
|
B.Set(i, 28, -H*H*r)
|
|
B.Set(i, 29, -L*P*H*r)
|
|
B.Set(i, 30, -L*L*L*r)
|
|
B.Set(i, 31, -L*P*P*r)
|
|
B.Set(i, 32, -L*H*H*r)
|
|
B.Set(i, 33, -L*L*P*r)
|
|
B.Set(i, 34, -P*P*P*r)
|
|
B.Set(i, 35, -P*H*H*r)
|
|
B.Set(i, 36, -L*L*H*r)
|
|
B.Set(i, 37, -P*P*H*r)
|
|
B.Set(i, 38, -H*H*H*r)
|
|
}
|
|
|
|
return B
|
|
}
|
|
|
|
// 构建权矩阵 [ 1/B ]
|
|
func setupWeightMatrix(coeffs, latVec, lonVec, heightVec *mat.VecDense) *mat.DiagDense {
|
|
n := latVec.Len()
|
|
row := mat.NewDense(n, 20, nil)
|
|
result := mat.NewDiagDense(n, nil)
|
|
for i := 0; i < n; i++ {
|
|
P := latVec.AtVec(i)
|
|
L := lonVec.AtVec(i)
|
|
H := heightVec.AtVec(i)
|
|
|
|
row.Set(i, 0, 1)
|
|
row.Set(i, 1, L)
|
|
row.Set(i, 2, P)
|
|
row.Set(i, 3, H)
|
|
row.Set(i, 4, L*P)
|
|
row.Set(i, 5, L*H)
|
|
row.Set(i, 6, P*H)
|
|
row.Set(i, 7, L*L)
|
|
row.Set(i, 8, P*P)
|
|
row.Set(i, 9, H*H)
|
|
row.Set(i, 10, L*P*H)
|
|
row.Set(i, 11, L*L*L)
|
|
row.Set(i, 12, L*P*P)
|
|
row.Set(i, 13, L*H*H)
|
|
row.Set(i, 14, L*L*P)
|
|
row.Set(i, 15, P*P*P)
|
|
row.Set(i, 16, P*H*H)
|
|
row.Set(i, 17, L*L*H)
|
|
row.Set(i, 18, P*P*H)
|
|
row.Set(i, 19, H*H*H)
|
|
|
|
var B float64
|
|
for idx2 := 0; idx2 < 20; idx2++ {
|
|
B += coeffs.AtVec(idx2) * row.At(i, idx2)
|
|
}
|
|
|
|
result.SetDiag(i, 1/B)
|
|
}
|
|
|
|
return result
|
|
}
|
|
|
|
func invertRPCMatrix(At *mat.Dense) (*mat.Dense, error) {
|
|
var AtInv mat.Dense
|
|
err := AtInv.Inverse(At)
|
|
|
|
if err != nil {
|
|
// 岭估计方法调整法方程状态，使得矩阵非奇异，最小二乘平差可以收敛
|
|
r, c := At.Dims()
|
|
log.Debugf("cannot inverse matrix(%d*%d): %v", r, c, err)
|
|
k := 0.0000001 // [0.00000005, 0.000005]
|
|
log.Debugf("try to adjust matrix with +kI, k=%.8f", k)
|
|
I := mat.NewDiagDense(r, nil)
|
|
for i := 0; i < r; i++ {
|
|
I.SetDiag(i, k)
|
|
}
|
|
At.Add(At, I)
|
|
|
|
err = AtInv.Inverse(At)
|
|
}
|
|
|
|
if err != nil {
|
|
log.Debugf("cannot inverse matrix: %v, try SVD method", err)
|
|
// 计算矩阵的 SVD 分解
|
|
var svd mat.SVD
|
|
ok := svd.Factorize(At, mat.SVDThin)
|
|
if !ok {
|
|
fmt.Println("SVD 分解失败")
|
|
return nil, fmt.Errorf("设计矩阵不可逆, SVD 分解失败: %v", err)
|
|
}
|
|
|
|
// 获取 U、Σ 和 V^T
|
|
var u, v mat.Dense
|
|
svd.UTo(&u)
|
|
svd.VTo(&v)
|
|
sigma := svd.Values(nil)
|
|
|
|
// 计算 Σ^+ (Sigma pseudo-inverse)
|
|
m, n := u.Dims()
|
|
sigmaInv := mat.NewDense(n, m, nil)
|
|
for i := 0; i < len(sigma); i++ {
|
|
if sigma[i] > 1e-10 { // 避免除以零
|
|
sigmaInv.Set(i, i, 1/sigma[i])
|
|
}
|
|
}
|
|
|
|
// 计算 V * Σ^+ * U^T
|
|
var temp mat.Dense
|
|
temp.Mul(&v, sigmaInv)
|
|
AtInv.Mul(&temp, u.T())
|
|
}
|
|
|
|
return &AtInv, nil
|
|
}
|
|
|
|
// SolveNormalEquation 使用正规方程法求解最小二乘问题
|
|
func SolveNormalEquation(A *mat.Dense, b *mat.VecDense) ([]float64, error) {
|
|
var At mat.Dense
|
|
At.Mul(A.T(), A) // At = A^T * A
|
|
|
|
// 求解 (A^T * A)^-1 * (A^T * b)
|
|
var AtInv mat.Dense
|
|
err := AtInv.Inverse(&At)
|
|
|
|
if err != nil {
|
|
// 岭估计方法调整法方程状态，使得矩阵非奇异，最小二乘平差可以收敛
|
|
r, c := At.Dims()
|
|
log.Infof("cannot inverse design matrix(%d*%d): %v", r, c, err)
|
|
log.Info("try to adjust design matrix with +kI, k=0.0000001")
|
|
k := 0.0000001 // [0.00000005, 0.000005]
|
|
I := mat.NewDiagDense(r, nil)
|
|
for i := 0; i < r; i++ {
|
|
I.SetDiag(i, k)
|
|
}
|
|
At.Add(&At, I)
|
|
|
|
err = AtInv.Inverse(&At)
|
|
}
|
|
|
|
if err != nil {
|
|
log.Infof("cannot inverse design matrix: %v, try SVD method", err)
|
|
// 计算矩阵的 SVD 分解
|
|
var svd mat.SVD
|
|
ok := svd.Factorize(&At, mat.SVDThin)
|
|
if !ok {
|
|
fmt.Println("SVD 分解失败")
|
|
return nil, fmt.Errorf("设计矩阵不可逆, SVD 分解失败: %v", err)
|
|
}
|
|
|
|
// 获取 U、Σ 和 V^T
|
|
var u, v mat.Dense
|
|
svd.UTo(&u)
|
|
svd.VTo(&v)
|
|
sigma := svd.Values(nil)
|
|
|
|
// 计算 Σ^+ (Sigma pseudo-inverse)
|
|
m, n := u.Dims()
|
|
sigmaInv := mat.NewDense(n, m, nil)
|
|
for i := 0; i < len(sigma); i++ {
|
|
if sigma[i] > 1e-10 { // 避免除以零
|
|
sigmaInv.Set(i, i, 1/sigma[i])
|
|
}
|
|
}
|
|
|
|
// 计算 V * Σ^+ * U^T
|
|
var temp mat.Dense
|
|
temp.Mul(&v, sigmaInv)
|
|
AtInv.Mul(&temp, u.T())
|
|
}
|
|
|
|
var Atb mat.VecDense
|
|
Atb.MulVec(A.T(), b) // Atb = A^T * b
|
|
|
|
var x mat.VecDense
|
|
x.MulVec(&AtInv, &Atb) // x = (A^T * A)^-1 * (A^T * b)
|
|
|
|
return mat.Col(nil, 0, &x), nil
|
|
}
|
|
|
|
func localize(num, den *mat.VecDense, row, col float64) (P, L, H float64) {
|
|
|
|
return
|
|
}
|
|
|
|
func project(num, den *mat.VecDense, P, L, H float64) (v float64) {
|
|
v = applyPolynominal(num, P, L, H) / applyPolynominal(den, P, L, H)
|
|
return v
|
|
}
|
|
|
|
func applyPolynominal(poly *mat.VecDense,
|
|
P, L, H float64) (v float64) {
|
|
v = 0.0
|
|
v += poly.AtVec(0)
|
|
v += poly.AtVec(1) * L
|
|
v += poly.AtVec(2) * P
|
|
v += poly.AtVec(3) * H
|
|
v += poly.AtVec(4) * L * P
|
|
v += poly.AtVec(5) * L * H
|
|
v += poly.AtVec(6) * P * H
|
|
v += poly.AtVec(7) * L * L
|
|
v += poly.AtVec(8) * P * P
|
|
v += poly.AtVec(9) * H * H
|
|
v += poly.AtVec(10) * P * L * H
|
|
v += poly.AtVec(11) * L * L * L
|
|
v += poly.AtVec(12) * L * P * P
|
|
v += poly.AtVec(13) * L * H * H
|
|
v += poly.AtVec(14) * L * L * P
|
|
v += poly.AtVec(15) * P * P * P
|
|
v += poly.AtVec(16) * P * H * H
|
|
v += poly.AtVec(17) * L * L * H
|
|
v += poly.AtVec(18) * P * P * H
|
|
v += poly.AtVec(19) * H * H * H
|
|
return v
|
|
}
|