Public Type ecef
x As Double
y As Double
z As Double
End Type
' 定义结构体存储ENU坐标
Public Type enu
e As Double
n As Double
u As Double
End Type
' 定义结构体存储观测点
Public Type ObservationPoint
ecef As ecef
enu As enu
End Type
Private Sub CommandButton1_Click()
' 定义结构体存储ECEF坐标
Dim Obs1(1 To 50) As ObservationPoint
Dim target As ecef
Dim RR As Double, RR1 As Double, JD As Double, L As Long, Nobs As Long
Dim Alpha As Double
With Sheet5
RR = 30000000
Nobs = .Cells(1, 4)
' 设置观测点1的ECEF坐标和ENU坐标
For i = 1 To Nobs
Obs1(i).ecef.x = .Cells(2 * (i - 1) + 1, 1)
Obs1(i).ecef.y = .Cells(2 * (i - 1) + 1, 2)
Obs1(i).ecef.z = .Cells(2 * (i - 1) + 1, 3)
Next i
L = 0
Alpha = .Cells(1, 5)
JD = .Cells(1, 6)
100 If L <= 500000 Then
For ik = 1 To Nobs
Obs1(ik).enu.e = .Cells(2 * ik, 1) * RR * Alpha
Obs1(ik).enu.n = .Cells(2 * ik, 2) * RR * Alpha
Obs1(ik).enu.u = .Cells(2 * ik, 3) * RR * Alpha
Next ik
' 计算目标点ECEF坐标
target = CalculateTargetECEF(Obs1(), Nobs)
RR1 = Sqr(target.x ^ 2 + target.y ^ 2 + target.z ^ 2)
If Abs((RR - RR1) / RR1) > JD Then
RR = RR1
L = L + 1
GoTo 100
End If
End If
.Cells(3, 8) = "L=" & L
' 输出结果
.Cells(1, 8) = "Target ECEF coordinates:"
.Cells(2, 8) = target.x
.Cells(2, 9) = target.y
.Cells(2, 10) = target.z
End With
End Sub
' 主函数：计算目标点ECEF坐标
Function CalculateTargetECEF(Obs1() As ObservationPoint, Nobs As Long) As ecef
Dim A(1 To 150, 1 To 3) As Double
Dim b(1 To 150) As Double
Dim i As Integer
' 构建设计矩阵A和观测向量b
For i = 1 To Nobs
Dim R(1 To 3, 1 To 3) As Double
Dim obs As ObservationPoint
obs = Obs1(i)
' 计算旋转矩阵
CalculateRotationMatrix obs.ecef, R
' 填充A矩阵
A(3 * (i - 1) + 1, 1) = R(1, 1)
A(3 * (i - 1) + 1, 2) = R(1, 2)
A(3 * (i - 1) + 1, 3) = R(1, 3)
A(3 * (i - 1) + 2, 1) = R(2, 1)
A(3 * (i - 1) + 2, 2) = R(2, 2)
A(3 * (i - 1) + 2, 3) = R(2, 3)
A(3 * (i - 1) + 3, 1) = R(3, 1)
A(3 * (i - 1) + 3, 2) = R(3, 2)
A(3 * (i - 1) + 3, 3) = R(3, 3)
' 填充b向量
b(3 * (i - 1) + 1) = R(1, 1) * obs.ecef.x + R(1, 2) * obs.ecef.y + R(1, 3) * obs.ecef.z + obs.enu.e
b(3 * (i - 1) + 2) = R(2, 1) * obs.ecef.x + R(2, 2) * obs.ecef.y + R(2, 3) * obs.ecef.z + obs.enu.n
b(3 * (i - 1) + 3) = R(3, 1) * obs.ecef.x + R(3, 2) * obs.ecef.y + R(3, 3) * obs.ecef.z + obs.enu.u
Next i
' 使用最小二乘法求解
CalculateTargetECEF = LeastSquares(A, b, Nobs)
End Function
' 计算旋转矩阵（ECEF到ENU）
Sub CalculateRotationMatrix(ecef As ecef, ByRef R() As Double)
Dim lat As Double, lon As Double
Dim A As Double, e2 As Double
Dim h As Double
h = 0
' WGS84椭球参数
A = 6378137# ' 半长轴
e2 = 0.00669437999014 ' 第一偏心率的平方
' 计算经纬度
Call ECEFToGeodetic(ecef, lat, lon, h, A, e2)
h = h + A
' 计算旋转矩阵
R(1, 1) = -Sin(lon)
R(1, 2) = -Sin(lat) * Cos(lon)
R(1, 3) = Cos(lat) * Cos(lon)
R(2, 1) = Cos(lon)
R(2, 2) = -Sin(lat) * Sin(lon)
R(2, 3) = Cos(lat) * Sin(lon)
R(3, 1) = 0
R(3, 2) = Cos(lat)
R(3, 3) = Sin(lat)
End Sub
' ECEF转大地坐标（简化实现）
Sub ECEFToGeodetic(ecef As ecef, ByRef lat As Double, ByRef lon As Double, ByRef height As Double, A As Double, e2 As Double)
Dim p As Double, theta As Double
Dim n As Double
p = Sqr(ecef.x * ecef.x + ecef.y * ecef.y)
lon = Atn2(ecef.y, ecef.x)
' 迭代计算纬度
lat = Atn2(ecef.z, p * (1 - e2))
Do
n = A / Sqr(1 - e2 * Sin(lat) * Sin(lat))
theta = Atn2(ecef.z + e2 * n * Sin(lat), p)
If Abs(lat - theta) < 0.0000000001 Then Exit Do
lat = theta
Loop
height = p / Cos(lat) - n
End Sub
' 反正切函数
Function Atn2(y As Double, x As Double) As Double
If x > 0 Then
Atn2 = Atn(y / x)
ElseIf x < 0 Then
Atn2 = Atn(y / x) + Sgn(y) * 3.14159265358979
Else
Atn2 = Sgn(y) * 3.14159265358979 / 2
End If
End Function
' 最小二乘法求解
Function LeastSquares(A() As Double, b() As Double, Nobs As Long) As ecef
Dim AT(1 To 3, 1 To 500) As Double
Dim ATA(1 To 3, 1 To 3) As Double
Dim ATb(1 To 3) As Double
Dim i As Integer, j As Integer, k As Integer
' 计算A的转置
For i = 1 To 3 * Nobs
For j = 1 To 3
AT(j, i) = A(i, j)
Next j
Next i
' 计算ATA = A^T * A
For i = 1 To 3
For j = 1 To 3
ATA(i, j) = 0
For k = 1 To 3 * Nobs
ATA(i, j) = ATA(i, j) + AT(i, k) * A(k, j)
Next k
Next j
Next i
' 计算ATb = A^T * b
For i = 1 To 3
ATb(i) = 0
For k = 1 To 3 * Nobs
ATb(i) = ATb(i) + AT(i, k) * b(k)
Next k
Next i
' 求解方程组ATA * x = ATb
' 这里使用高斯消元法
LeastSquares = SolveLinearSystem(ATA, ATb)
End Function
' 求解线性方程组（高斯消元法）
Function SolveLinearSystem(A() As Double, b() As Double) As ecef
Dim n As Integer: n = 3
Dim Augmented(1 To 3, 1 To 4) As Double
Dim x(1 To 3) As Double
Dim i As Integer, j As Integer, k As Integer
Dim factor As Double
' 构建增广矩阵
For i = 1 To n
For j = 1 To n
Augmented(i, j) = A(i, j)
Next j
Augmented(i, n + 1) = b(i)
Next i
' 前向消元
For i = 1 To n - 1
For j = i + 1 To n
factor = Augmented(j, i) / Augmented(i, i)
For k = i To n + 1
Augmented(j, k) = Augmented(j, k) - factor * Augmented(i, k)
Next k
Next j
Next i
' 回代
x(n) = Augmented(n, n + 1) / Augmented(n, n)
For i = n - 1 To 1 Step -1
x(i) = Augmented(i, n + 1)
For j = i + 1 To n
x(i) = x(i) - Augmented(i, j) * x(j)
Next j
x(i) = x(i) / Augmented(i, i)
Next i
' 返回结果
SolveLinearSystem.x = x(1)
SolveLinearSystem.y = x(2)
SolveLinearSystem.z = x(3)
End Function
转载本文请联系原作者获取授权，同时请注明本文来自刘志明科学网博客。
链接地址：https://wap.sciencenet.cn/blog-3556836-1499370.html?mobile=1
收藏
