From df6090df217ec73003f6871040719eb1cec55c50 Mon Sep 17 00:00:00 2001
From: nuknal <linktolane@gmail.com>
Date: Wed, 4 Sep 2024 17:59:12 +0800
Subject: [PATCH] =?UTF-8?q?=E6=8E=A7=E5=88=B6=E7=82=B9=E8=BF=87=E5=B0=91?=
 =?UTF-8?q?=E6=97=B6=E5=AF=BC=E8=87=B4=E5=9B=BE=E5=83=8F=E5=86=85=E7=95=B8?=
 =?UTF-8?q?=E5=8F=98=E8=BE=83=E5=A4=A7?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 pkg/producer/rpc.go          | 12 ++++++------
 pkg/producer/rpc_helper.go   | 29 +++++++++++------------------
 pkg/producer/rpc_optimize.go |  3 ---
 3 files changed, 17 insertions(+), 27 deletions(-)

diff --git a/pkg/producer/rpc.go b/pkg/producer/rpc.go
index 2e617de..a107b8c 100644
--- a/pkg/producer/rpc.go
+++ b/pkg/producer/rpc.go
@@ -57,7 +57,7 @@ type RPCModel struct {
 func NewRPC(r *Registrator, scene *Scene, rpb string) *RPC {
 	rpc := RPC{
 		elevationLayer: 3,
-		gridsize:       3,
+		gridsize:       19,
 		registrator:    r,
 		scene:          scene,
 		rpb:            rpb,
@@ -150,14 +150,14 @@ func (rpc *RPC) RPC() error {
 	rpc.saveVec(strings.Replace(rpc.scene.Tiff, ".tiff", ".vec_norm.txt", -1),
 		rowVec, colVec, latVec, lonVec, heightVec)
 
-	// 设计矩阵 B = [ 20个分子系数 19个分母系数 ]
-
-	// x = (B^T * B)^-1 * B^T * l, 其中 x = [a1..a20 b2..b20]^T
+	// 设计矩阵 B = [ 20个分子系数 19个分母系数 ] W = 权矩阵 R = 观测结果
+	method := SolveMethodNelderMead
+	// x = (B^T * W^2 * B)^-1 * (B^T * W^2 * R), 其中 x = [a1..a20 b2..b20]^T
 	// 行参数
 	// B := setupSystemOfEquations(rowVec, latVec, lonVec, heightVec)
 	// J, err := SolveNormalEquation(B, rowVec)
 	log.Println("solving row coefficients")
-	J, err := solveCoefficients(rowVec, latVec, lonVec, heightVec, SolveMethodNelderMead)
+	J, err := solveCoefficients(rowVec, latVec, lonVec, heightVec, method)
 	if err != nil {
 		return err
 	}
@@ -166,7 +166,7 @@ func (rpc *RPC) RPC() error {
 	// D := setupSystemOfEquations(colVec, latVec, lonVec, heightVec)
 	// K, err := SolveNormalEquation(D, colVec)
 	log.Println("solving col coefficients")
-	K, err := solveCoefficients(colVec, latVec, lonVec, heightVec, SolveMethodNelderMead)
+	K, err := solveCoefficients(colVec, latVec, lonVec, heightVec, method)
 	if err != nil {
 		return err
 	}
diff --git a/pkg/producer/rpc_helper.go b/pkg/producer/rpc_helper.go
index 213b126..00b8533 100644
--- a/pkg/producer/rpc_helper.go
+++ b/pkg/producer/rpc_helper.go
@@ -46,28 +46,18 @@ func normalize2(v *mat.VecDense, vOff, vScale float64) *mat.VecDense {
 func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense, method SolveMethod) ([]float64, error) {
 	M := setupSystemOfEquations(f, latVec, lonVec, heightVec)
 	n := f.Len()
-	weights := mat.NewDiagDense(n, nil)
-	for i := 0; i < n; i++ {
-		weights.SetDiag(i, 1.0)
-	}
 
-	// 计算初始值
+	// 计算初始值 x = (M^T * M)^-1 * (M^T * f)
 	var x0 mat.VecDense
-	w2 := mat.NewDiagDense(n, nil)
-	for i := 0; i < n; i++ {
-		w2.SetDiag(i, weights.At(i, i)*weights.At(i, i))
-	}
-	var MtW2 mat.Dense
-	MtW2.Mul(M.T(), w2)
-	var MtW2M mat.Dense
-	MtW2M.Mul(&MtW2, M)
-	invMtW2M, err := invertRPCMatrix(&MtW2M)
+	var MtM mat.Dense
+	MtM.Mul(M.T(), M)
+	invMtM, err := invertRPCMatrix(&MtM)
 	if err != nil {
 		return nil, err
 	}
-	var MtW2F mat.VecDense
-	MtW2F.MulVec(&MtW2, f)
-	x0.MulVec(invMtW2M, &MtW2F)
+	var MtF mat.VecDense
+	MtF.MulVec(M.T(), f)
+	x0.MulVec(invMtM, &MtF)
 
 	if method == SolveMethodNelderMead {
 		numerator := mat.NewVecDense(20, nil)
@@ -102,12 +92,15 @@ func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense, method SolveM
 	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)
+		weights := setupWeightMatrix(denominator, latVec, lonVec, heightVec)
 		for i := 0; i < n; i++ {
 			w2.SetDiag(i, weights.At(i, i)*weights.At(i, i))
 		}
diff --git a/pkg/producer/rpc_optimize.go b/pkg/producer/rpc_optimize.go
index 51474d4..7c14385 100644
--- a/pkg/producer/rpc_optimize.go
+++ b/pkg/producer/rpc_optimize.go
@@ -1,8 +1,6 @@
 package producer
 
 import (
-	"fmt"
-
 	log "github.com/sirupsen/logrus"
 	"gonum.org/v1/gonum/mat"
 	"gonum.org/v1/gonum/optimize"
@@ -54,7 +52,6 @@ func solveNelderMead(num, den, f, latVec, lonVec, heightVec *mat.VecDense) (*mat
 	denominator := mat.NewVecDense(20, nil)
 	denominator.SetVec(0, 1.0)
 	numerator.SetVec(0, result.X[0])
-	fmt.Println("len of result.X: ", len(result.X))
 	for i := 1; i < 20; i++ {
 		numerator.SetVec(i, result.X[i])
 		denominator.SetVec(i, result.X[i+19])